Diff: main.cpp

Revision:

5:e4b9cc904928

Parent:

4:db3dad7e53e3

Child:

6:f260176f704b

--- a/main.cpp	Wed Oct 22 13:23:30 2014 +0000
+++ b/main.cpp	Wed Oct 22 14:08:43 2014 +0000
@@ -20,7 +20,7 @@
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
 volatile bool looptimerflag;
-HIDScope scope(6);
+HIDScope scope(2);
 
 
 void setlooptimerflag(void)
@@ -52,46 +52,24 @@
     while(1) {
         pwm_motor1.write(1);
         motordir1.write(1);
-        do {
+
+        wait(0.01);
+        scope.set(0, motor1.getposition());
+        scope.set(1, motor2.getPosition());
+        scope.send();
+        /*do {
 
             if(pc.readable()) {
                 c = pc.getc();
             }
-        } 
-        while((c !='1'));
-        {
-            c = '0';
-            pwm_motor1.write(0.5);
-            motordir1.write(0);
-            wait(3);
-        }
-    }
-}
+        } while((c !='1'));
 
-//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;
+        c = '0';
+        pwm_motor1.write(0.5);
+        motordir1.write(0);
+        wait(3);
+
+    }*/
 }
 
 
-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;
-}