Diff: main.cpp

Revision:

21:c856b7752d7d

Parent:

19:3e46c457091c

Child:

22:22cb158bcea4

--- a/main.cpp	Thu Oct 30 11:45:48 2014 +0000
+++ b/main.cpp	Thu Oct 30 12:18:56 2014 +0000
@@ -47,7 +47,7 @@
 
 
 
-
+/*
 volatile bool looptimerflag;
 
 bool flip=false;
@@ -64,9 +64,27 @@
     scope.set(0, motor1.getPosition());
     scope.set(1, motor2.getPosition());
     scope.send();
+}*/
+
+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 cur_pos_motor1)
+{
+    float error;
+    float           out_p = 0;
+    static float    out_i = 0;
+    error  = (setpoint - cur_pos_motor1);
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    return out_p + out_i;
+    }
 int main()
 {
     //Ticker log_timer;
@@ -75,7 +93,7 @@
     pwm_motor1.period_us(100);
     float prev_setpoint = 0;
     float setpoint;
-    speed1_rad = 0.29;
+    speed1_rad = 0.05;
 
 
     while(1) {
@@ -100,8 +118,8 @@
                 motordir1 = 0;
             }
 
-        if(cur_pos_motor1 < 1032) {
-            pwm_motor1.write(pwm1_percentage/100);//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
+        if(cur_pos_motor1 < 130) {
+            pwm_motor1.write(abs(pwm1_percentage/100));//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
             prev_setpoint = setpoint;
         } 
         else {
@@ -110,22 +128,5 @@
     }
 }
 
-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;
-    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);
-    return out_p + out_i;
-}