Diff: main.cpp

Revision:

33:0417920456d0

Parent:

32:c6f453a2fd3e

Child:

34:1cea2a17e961

--- a/main.cpp	Fri Oct 31 14:01:28 2014 +0000
+++ b/main.cpp	Fri Oct 31 14:15:10 2014 +0000
@@ -142,14 +142,14 @@
 {
     speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (155.0*2.0*PI/360);
+    if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (360.0*2.0*PI/360);
     }
-    if(setpoint2 < -(155.0*2.0*PI/360)) {
-        setpoint2 = -(155.0*2.0*PI/360);
+    if(setpoint2 < -(360.0*2.0*PI/360)) {
+        setpoint2 = -(360.0*2.0*PI/360);
     }
     prev_setpoint2 = setpoint2;
-    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+    if(setpoint2 >= (360.0*2.0*PI/360)-0.1) {
         staat2 = 1;
     }
 }
@@ -188,20 +188,21 @@
 
 void arm_begin ()
 {
-    speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
+    speed2_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (155.0*2.0*PI/360);
+    if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
+        setpoint2 = (0.0*2.0*PI/360);
     }
-    if(setpoint2 < -(155.0*2.0*PI/360)) {
-        setpoint2 = -(155.0*2.0*PI/360);
+    if(setpoint2 < -(0.0*2.0*PI/360)) {
+        setpoint2 = -(0.0*2.0*PI/360);
     }
     prev_setpoint2 = setpoint2;
 }
 
 int main()
 {
-    int wait_iterator = 0;
+    int wait_iterator1 = 0;
+    int wait_iterator2 = 0;
     pwm_motor1.period_us(100);
     motor1.setPosition(0);
     pwm_motor2.period_us(100);
@@ -243,14 +244,16 @@
         } else {
             motordir2 = 1;
         }
+        
         wait(TSAMP);
+       
         //STATES
         if(staat1 == 0) {
             batje_rechts();
-            wait_iterator = 0;
+            wait_iterator1 = 0;
         } else if(staat1 ==1) {
-            wait_iterator++;
-            if(wait_iterator > 600)
+            wait_iterator1++;
+            if(wait_iterator1 > 600)
                 staat1 = 2;
         } else {
             batje_begin_rechts();
@@ -259,10 +262,10 @@
 
         if(staat2 == 0) {
             arm_hoog();
-            wait_iterator = 0;
-        } else if(staat2 ==1) {
-            wait_iterator++;
-            if(wait_iterator > 300)
+            wait_iterator2 = 0;
+        } else if(staat2 == 1) {
+            wait_iterator2++;
+            if(wait_iterator2 > 200)
                 staat2 = 2;
         } else {
             arm_begin();