Diff: main.cpp

Revision:

26:ce2d9945e45d

Parent:

25:28be2bf11ad7

--- a/main.cpp	Fri Oct 31 15:21:20 2014 +0000
+++ b/main.cpp	Fri Oct 31 16:15:54 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.02)
-#define K_Ip (0.002  *TSAMP1)
+#define K_Pp (0.2)
+#define K_Ip (0.02  *TSAMP1)
 #define K_Dp (0  /TSAMP1)
 
 #define TSAMP1 0.01
@@ -675,7 +675,6 @@
         pc.printf("toestand = terugkeren\n\r");
     }
     if (toestand == WACHTEN) {
-        pidpositie(ANGLEMIN, motor1.getPosition());
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
     }
@@ -684,19 +683,28 @@
         pwm_motor1.write(new_pwm);
         motordir1 = 1;
         pc.printf("SLAAN\n");
-        
+    }
+
     if (toestand == TERUGKEREN) {
-        pidpositie(ANGLEMAX, motor1.getPosition());
+        new_pos = pidpositie(ANGLEMAX, motor1.getPosition());
+        if (ANGLEMAX <= motor1.getPosition()) {
+            motordir1 = 1;
+            pwm_motor1.write(new_pos);
+        } else{
+            motordir1 = 0;
+            pwm_motor1.write(new_pos);
         }
+    }
 
 
-        /*if (toestand == TERUGKEREN) {
-            new_pwm = pid(setspeed, motor1.getSpeed(),false);
-            pwm_motor1.write(new_pwm);
-            motordir1 = 0;
-            pc.printf("motor gaat terugkeren\n\r");
-            pc.printf("new pwm %f\r\n",new_pwm);*/
-    }
+
+    /*if (toestand == TERUGKEREN) {
+        new_pwm = pid(setspeed, motor1.getSpeed(),false);
+        pwm_motor1.write(new_pwm);
+        motordir1 = 0;
+        pc.printf("motor gaat terugkeren\n\r");
+        pc.printf("new pwm %f\r\n",new_pwm);}*/
+
     scope.set(0,motor1.getPosition());        //ruwe data
     scope.set(1,motor1.getPosition());     //filtered
     scope.send();
@@ -721,8 +729,14 @@
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
         new_pos = pidpositie(ANGLEMIN, motor1.getPosition());
+        if (ANGLEMIN <= motor1.getPosition()) {
+            motordir1 = 1;
+            pwm_motor1.write(new_pos);
+        } else {
+            motordir1 = 0;
+            pwm_motor1.write(new_pos);
+        }
         pc.printf("new position %f/r/n", new_pos);
-        pwm_motor1.write(new_pos);
         //pc.printf("ifwachten2\n");
     }
     //sturen naar HID Scope