Diff: main.cpp

Revision:

31:0309fbf7b71c

Parent:

30:6ace856e0ad7

Child:

32:c6f453a2fd3e

--- a/main.cpp	Fri Oct 31 13:34:40 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:38:19 2014 +0000
@@ -83,8 +83,6 @@
 void batje_links ()
 {
     speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
-    cur_pos_motor1 = motor1.getPosition();
-    pos_motor1_rad = (float)cur_pos_motor1/(3600.0/(2.0*PI));
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
     if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (180*2.3*2.0*PI/360);
@@ -92,11 +90,7 @@
     if(setpoint1 < -(180*2.3*2.0*PI/360)) {
         setpoint1 = -(180*2.3*2.0*PI/360);
     }
-
-    pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
-    wait(TSAMP);
-
 }
 
 void batje_rechts ()
@@ -114,9 +108,32 @@
     if(setpoint1 > (360*2.3*2.0*PI/360)) {
         staat1 = 1;
     }
-    
+}
 
+void batje_begin_links ()
+{
+    speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (180*2.3*2.0*PI/360);
+    }
+    if(setpoint1 < -(180*2.3*2.0*PI/360)) {
+        setpoint1 = -(180*2.3*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
+}
 
+void batje_begin_rechts ()
+{
+    speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (180*2.3*2.0*PI/360);
+    }
+    if(setpoint1 < -(180*2.3*2.0*PI/360)) {
+        setpoint1 = -(180*2.3*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
 }
 
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
@@ -129,13 +146,10 @@
     if(setpoint1 < -(155.0*2.0*PI/360)) {
         setpoint1 = -(155.0*2.0*PI/360);
     }
-    pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
     if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
      staat1 = 1;
     }
-    
-
 }
 
 void arm_mid ()