Diff: main.cpp

Revision:

0:c9e647421e54

Child:

1:5ac85aad9da4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 26 11:31:08 2013 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+#define K_P 0.015//184997836671646 
+#define K_I 0.//000824387821287097
+#define K_D 0.077//972091946803081
+
+void coerce(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main() {
+    Encoder motor1(PTD0,PTC9);
+    MODSERIAL pc(USBTX,USBRX);
+    PwmOut pwm_motor(PTA12);
+    DigitalOut motordir(PTD3);
+    AnalogIn   potmeter(PTC2);
+    pc.baud(115200);
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);   
+    while(1) {
+        static float setpoint = 0;
+        float new_pwm;
+        while(!looptimerflag);
+        looptimerflag = false;
+        setpoint = (potmeter.read()-0.5)*2000;
+        new_pwm = pid(setpoint, motor1.getPosition());
+        coerce(&new_pwm, -1,1);
+        if(new_pwm > 0)
+            motordir = 1;
+        else
+            motordir = 0;  
+        pwm_motor.write(abs(new_pwm));
+    }
+}
+
+void coerce(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+
+float pid(float setpoint, float measurement)
+{
+        //static int16_t setpoint = 0            //static int16_t setpoint = 0      
+        float error;
+        static float prev_error = 0;
+        float           out_p = 0;
+        static float    out_i = 0;
+        float           out_d = 0;
+        error  = setpoint-measurement;
+        out_p  = error*K_P;
+        out_i += error*K_I;
+        out_d = (error-prev_error)*K_D;
+        coerce(&out_i,-0.5,0.5);
+        prev_error = error;
+        return out_p + out_i + out_d;    
+}
+