Diff: main.cpp

Revision:

18:fb85c58a4106

Parent:

7:697293226e5e

Child:

19:3e46c457091c

--- a/main.cpp	Thu Oct 23 10:47:35 2014 +0000
+++ b/main.cpp	Thu Oct 30 09:42:58 2014 +0000
@@ -7,6 +7,10 @@
 #define K_I (0.03  *TSAMP)
 #define K_D (0.001 /TSAMP)
 #define I_LIMIT 1.
+#define pwm1_min 0
+#define pwm2_min 0
+#define PI 3.14159265
+#define l_arm 0.5
 
 #define M1_PWM PTC8
 #define M1_DIR PTC9
@@ -28,6 +32,22 @@
 Encoder motor2(PTD2, PTD0); //wit, geel
 PwmOut pwm_motor2(M1_PWM);
 DigitalOut motordir2(M1_DIR);
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+
+float pwm1_percentage = 0;
+float pwm2_percentage = 0;
+int cur_pos_motor1;
+int prev_pos_motor1 = 0;
+int cur_pos_motor2;
+int prev_pos_motor2 = 0;
+float speed1_rad;
+float speed2_rad; 
+
+
+
+
+volatile bool looptimerflag;
 
 bool flip=false;
 
@@ -49,10 +69,31 @@
 int main()
 {
     Ticker log_timer;
-    Ticker timer;
     log_timer.attach(looper, TSAMP);
-    timer.attach(&attime, 7);
+
     while(1) {
 
     }
 }
+
+void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
+// maar de locatie van de variabele. 
+{
+    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
+    // *in = het getal dat staat op locatie van in --> waarde van new_pwm
+}
+
+
+float pid(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    error  = (setpoint-measurement);
+    out_p  = error*K_P; 
+    out_i += error*K_I;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    return out_p + out_i + out_d;
+}