Diff: main.cpp

Revision:

34:1cea2a17e961

Parent:

33:0417920456d0

--- a/main.cpp	Fri Oct 31 14:15:10 2014 +0000
+++ b/main.cpp	Fri Oct 31 14:39:47 2014 +0000
@@ -6,7 +6,7 @@
 #define TSAMP 0.005
 #define K_P1 (3.5) //voor motor1 is het 3.5
 #define K_I1 (0.01  *TSAMP) //voor motor1 is het 0.01
-#define K_P2 (1.0)
+#define K_P2 (3.5)
 #define K_I2 (0.01 *TSAMP)
 #define I_LIMIT 1.
 #define PI 3.14159265
@@ -140,7 +140,7 @@
 
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
 {
-    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
+    speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
     if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden
         setpoint2 = (360.0*2.0*PI/360);
@@ -245,7 +245,6 @@
             motordir2 = 1;
         }
         
-        wait(TSAMP);
        
         //STATES
         if(staat1 == 0) {
@@ -270,5 +269,6 @@
         } else {
             arm_begin();
         }
+        wait(TSAMP);
     }
 }