Diff: main.cpp

Revision:

0:d5d3b731340c

Child:

1:97d6b160f708

diff -r 000000000000 -r d5d3b731340c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+
+#define TSAMP 0.01
+#define K_P (0.1)
+#define K_I (0.03  *TSAMP)
+#define K_D (0.001 /TSAMP)
+#define I_LIMIT 1.
+
+#define M1_PWM PTC8 //blauw
+#define M1_DIR PTC9 //groen
+#define M2_PWM PTA5
+#define M2_DIR PTA4
+
+
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+volatile bool looptimerflag;
+HIDScope scope(6);
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main()
+{
+    //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
+    Encoder motor1(PTD3,PTD5); //wit, geel
+    /*PwmOut to motor driver*/
+    PwmOut pwm_motor(M1_PWM); // PTC8, blauw
+    //10kHz PWM frequency
+    pwm_motor.period_us(75);
+    DigitalOut motordir(M1_DIR); //PTC9, groen
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,TSAMP);
+    while(1) {
+        int16_t 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;
+}