Diff: main.cpp

Revision:

30:28be2bf11ad7

Parent:

24:a165dcd86710

Child:

31:ce2d9945e45d

--- a/main.cpp	Fri Oct 31 14:54:49 2014 +0000
+++ b/main.cpp	Fri Oct 31 15:21:20 2014 +0000
@@ -9,8 +9,8 @@
 #define K_I (0.02  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
-#define K_Pp (0.2)
-#define K_Ip (0.02  *TSAMP1)
+#define K_Pp (0.02)
+#define K_Ip (0.002  *TSAMP1)
 #define K_Dp (0  /TSAMP1)
 
 #define TSAMP1 0.01
@@ -81,6 +81,7 @@
 
 bool stop;
 float new_pwm;
+float new_pos;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
 int toestand = WACHTEN; //terugkeren?
 float setspeed = 0, V3=60, V2=40, V1 =30, Vreturn= 35;//V in counts/s
@@ -719,7 +720,9 @@
     }
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
-        pidpositie(ANGLEMIN, motor1.getPosition());
+        new_pos = pidpositie(ANGLEMIN, motor1.getPosition());
+        pc.printf("new position %f/r/n", new_pos);
+        pwm_motor1.write(new_pos);
         //pc.printf("ifwachten2\n");
     }
     //sturen naar HID Scope