Diff: main.cpp

Revision:

0:eb00992c1597

Child:

2:ef0fa691e77e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 03 08:56:01 2014 +0000
@@ -0,0 +1,88 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+
+#define TSAMP 0.01
+#define K_P (0.01)
+#define K_I (0  *TSAMP)
+#define K_D (0      /TSAMP)
+#define I_LIMIT 1.
+
+#define POT_AVG 50
+
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+AnalogIn potmeter(PTC2);
+volatile bool looptimerflag;
+float potsamples[POT_AVG];
+HIDScope scope(6);
+
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main()
+{
+    //start Encoder
+    Encoder motor1(PTD0,PTC9);
+    /*PwmOut to motor driver*/
+    PwmOut pwm_motor(PTA5);
+    //10kHz PWM frequency
+    pwm_motor.period_us(100);
+    DigitalOut motordir(PTD1);
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,TSAMP);
+    while(1) {
+        float setpoint;
+        float new_pwm;
+        /*wait until timer has elapsed*/
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+        /*potmeter value: 0-1*/
+        setpoint = (potmeter.read()-.5)*500;        
+        /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
+        new_pwm = pid(setpoint, motor1.getPosition());
+        clamp(&new_pwm, -1,1);
+        scope.set(0, setpoint);
+        scope.set(4, new_pwm);
+        scope.set(5, motor1.getPosition());
+        // ch 1, 2 and 3 set in pid controller */
+        scope.send();
+        if(new_pwm > 0)
+            motordir = 0;
+        else
+            motordir = 1;
+        pwm_motor.write(abs(new_pwm));
+    }
+}
+
+
+//clamps value 'in' to min or max when exceeding those values
+//if you'd like to understand the statement below take a google for
+//'ternary operators'.
+void clamp(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+
+float pid(float setpoint, float measurement)
+{
+    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;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    scope.set(1,out_p);
+    scope.set(2,out_i);
+    scope.set(3,out_d);
+    return out_p + out_i + out_d;
+}