Diff: main.cpp

Revision:

17:3ddab322341a

Parent:

16:f4df06bf58ab

--- a/main.cpp	Wed Oct 29 12:57:25 2014 +0000
+++ b/main.cpp	Wed Oct 29 15:06:18 2014 +0000
@@ -19,7 +19,7 @@
 
 Serial pc(USBTX, USBRX);
 DigitalOut led1(LED_RED);
-HIDScope scope(4);
+HIDScope scope(2);
 
 //motor 25D
 Encoder motor1(PTD3,PTD5,true); //wit, geel
@@ -45,31 +45,12 @@
 
 void looper()
 {
-    /*motordir1=0;
-    pwm_motor1.write(0.06);
+    wait(15);
+    motordir1=0;
+    pwm_motor1.write(1);
     scope.set(0, motor1.getPosition());
-    scope.set(1, motor1.getSpeed());
-    scope.set(2, motor2.getPosition());
-    scope.set(3, motor2.getSpeed());
-    scope.send();*/
-}
-
-void clamp(float * in, float min, float max)
-{
-*in > min ? *in < max? : *in = max: *in = min;
-}
-
-float pi(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;
+    scope.set(1, motor2.getPosition());
+    scope.send();
 }
 
 int main()
@@ -107,7 +88,7 @@
     */
 
     //motor2
-    wait(1);
+    /*wait(1);
     speed2 = 1;
     motordir1=1;
     pwm_motor1.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
@@ -122,7 +103,7 @@
     motordir1=1;            //CW
     pwm_motor1.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
     wait(1);            //terug naar begin positie, 20 graden
-    pwm_motor1.write(0); 
+    pwm_motor1.write(0); */
 
     
     pc.baud(115200);