Diff: main.cpp

Revision:

32:c6f453a2fd3e

Parent:

31:0309fbf7b71c

Child:

33:0417920456d0

--- a/main.cpp	Fri Oct 31 13:38:19 2014 +0000
+++ b/main.cpp	Fri Oct 31 14:01:28 2014 +0000
@@ -44,6 +44,7 @@
 float pos_motor1_rad;
 float pos_motor2_rad;
 int staat1 = 0;
+int staat2 = 0;
 
 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
 // maar de locatie van de variabele.
@@ -95,21 +96,22 @@
 
 void batje_rechts ()
 {
-    speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
+    speed1_rad = 3.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (360*2.3*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (360*2.3*2.0*PI/360);
+    if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (11.3*2.3*2.0*PI/360);
     }
     if(setpoint1 < -(360*2.3*2.0*PI/360)) {
         setpoint1 = -(360*2.3*2.0*PI/360);
     }
     pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
-    if(setpoint1 > (360*2.3*2.0*PI/360)) {
+    if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
         staat1 = 1;
     }
 }
 
+
 void batje_begin_links ()
 {
     speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
@@ -125,72 +127,76 @@
 
 void batje_begin_rechts ()
 {
-    speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
+    speed1_rad = -2.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);
+    if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.3*2.0*PI/360);
     }
     prev_setpoint1 = setpoint1;
 }
 
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
 {
-    speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (155.0*2.0*PI/360);
+    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(setpoint1 < -(155.0*2.0*PI/360)) {
-        setpoint1 = -(155.0*2.0*PI/360);
+    if(setpoint2 < -(155.0*2.0*PI/360)) {
+        setpoint2 = -(155.0*2.0*PI/360);
     }
-    prev_setpoint1 = setpoint1;
-    if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
-     staat1 = 1;
+    prev_setpoint2 = setpoint2;
+    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+        staat2 = 1;
     }
 }
 
 void arm_mid ()
 {
-    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
-    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (155*2.0*PI/360);
+    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(setpoint1 < -(0.0*2.0*PI/360)) {
-        setpoint1 = -(0.0*2.0*PI/360);
+    if(setpoint2 < -(155.0*2.0*PI/360)) {
+        setpoint2 = -(155.0*2.0*PI/360);
     }
-    prev_setpoint1 = setpoint1;
-    
-    //wait(TSAMP);
+    prev_setpoint2 = setpoint2;
+    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+        staat2 = 1;
+    }
 }
 
 void arm_laag ()
 {
-    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
-    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (155*2.0*PI/360);
+    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(setpoint1 < -(0.0*2.0*PI/360)) {
-        setpoint1 = -(0.0*2.0*PI/360);
+    if(setpoint2 < -(155.0*2.0*PI/360)) {
+        setpoint2 = -(155.0*2.0*PI/360);
     }
-    prev_setpoint1 = setpoint1;
+    prev_setpoint2 = setpoint2;
+    if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+        staat2 = 1;
+    }
 }
 
 void arm_begin ()
 {
-    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
-    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
-    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (155*2.0*PI/360);
+    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(setpoint1 < -(0.0*2.0*PI/360)) {
-        setpoint1 = -(0.0*2.0*PI/360);
+    if(setpoint2 < -(155.0*2.0*PI/360)) {
+        setpoint2 = -(155.0*2.0*PI/360);
     }
-    prev_setpoint1 = setpoint1;
+    prev_setpoint2 = setpoint2;
 }
 
 int main()
@@ -206,7 +212,7 @@
         //MOTOR1
         pc.printf("%d \r\n", motor1.getPosition());
         cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
         pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
         if (pwm1_percentage < -1.0) {
             pwm1_percentage = -1.0;
@@ -220,11 +226,11 @@
         } else {
             motordir1 = 1;
         }
-        
-         //MOTOR2
+
+        //MOTOR2
         cur_pos_motor2 = motor2.getPosition();
-        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); //moet 4128
-        pwm2_percentage = pid1(setpoint2, pos_motor2_rad);
+        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
+        pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
         if (pwm2_percentage < -1.0) {
             pwm2_percentage = -1.0;
         }
@@ -240,20 +246,26 @@
         wait(TSAMP);
         //STATES
         if(staat1 == 0) {
+            batje_rechts();
+            wait_iterator = 0;
+        } else if(staat1 ==1) {
+            wait_iterator++;
+            if(wait_iterator > 600)
+                staat1 = 2;
+        } else {
+            batje_begin_rechts();
+        }
+
+
+        if(staat2 == 0) {
             arm_hoog();
             wait_iterator = 0;
-        } 
-        else if(staat1 ==1)
-        {
+        } else if(staat2 ==1) {
             wait_iterator++;
-            if(wait_iterator > 200)
-                staat1 = 2;
-        }
-        else
-        {
+            if(wait_iterator > 300)
+                staat2 = 2;
+        } else {
             arm_begin();
         }
-       }
+    }
 }
-
-