Diff: main.cpp

Revision:

0:c4c11b0ac1b9

diff -r 000000000000 -r c4c11b0ac1b9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 03 03:57:23 2016 +0000
@@ -0,0 +1,82 @@
+#define MAPLE_MINI
+#include "mbed.h"
+#include "pines.h"
+#include <motoresDC.h>
+#include <HMC5883L.h>
+
+HMC5883L brujula;
+InterruptIn boton(PB_8);
+//motores       D27   D31    D30    D26   D29    D28     
+MotoresDC carro(PWM_L, CTRL1_L, CTRL2_L, PWM_R, CTRL1_R, CTRL2_R);
+
+Timer t;
+unsigned long prev_time;
+unsigned long interval = 33;
+double maxOut = 0.5;
+double minOut = -0.5;
+volatile double setpoint = 0.0;
+double iTerm = 0;
+double lastInput;
+double kp = 1.1;
+double ki = 0.01;
+double kd = 0.0;
+
+PwmOut led(PB_1);
+
+void toggle()
+{
+    if(setpoint == 0.0)
+    {
+        setpoint = 0.5;    
+    }
+    else
+    {
+        setpoint = 0.0;
+    }
+    
+}
+// functions 
+void init_p()
+{
+    led = 1;
+    carro.detener();
+    boton.fall(toggle);
+    brujula.init();
+    wait_ms(1000);
+    led = 0;
+    wait_ms(500);
+    led = 1;
+    t.start();
+    wait_ms(2000);
+    
+    led = 0;
+}
+
+int main() {
+    init_p();
+    while(1) {
+        unsigned long now = t.read_ms();
+        if((now - prev_time) > interval)
+        {
+            prev_time = now;
+            
+            double input = brujula.getHeading()/ 180.0 -1.0;
+            // PID
+            double error = setpoint - input;
+            if(abs(error) < 0.1) led = 1;
+            else led = 0;
+            
+            iTerm += ki * error;
+            if(iTerm > maxOut) iTerm = maxOut;
+            else if(iTerm < minOut) iTerm = minOut;
+            
+            double dInput = input - lastInput;
+            
+            double output = kp* error + iTerm + kd * dInput;
+            if(output > maxOut) output = maxOut;
+            else if(output < minOut) output = minOut;
+            carro.pivotar(output);   
+            lastInput = input;
+        }
+    }
+}