Diff: main.cpp

Revision:

30:6ace856e0ad7

Parent:

29:4b7f6f15cedf

Child:

31:0309fbf7b71c

--- a/main.cpp	Fri Oct 31 13:26:13 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:34:40 2014 +0000
@@ -43,7 +43,7 @@
 float speed2_rad;
 float pos_motor1_rad;
 float pos_motor2_rad;
-int staat = 0;
+int staat1 = 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.
@@ -86,11 +86,11 @@
     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.0*PI/360)) { //setpoint in graden
-        setpoint1 = (180*2.0*PI/360);
+    if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (180*2.3*2.0*PI/360);
     }
-    if(setpoint1 < -(180*2.0*PI/360)) {
-        setpoint1 = -(180*2.0*PI/360);
+    if(setpoint1 < -(180*2.3*2.0*PI/360)) {
+        setpoint1 = -(180*2.3*2.0*PI/360);
     }
 
     pwm_motor1.write(abs(pwm1_percentage));
@@ -102,21 +102,19 @@
 void batje_rechts ()
 {
     speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
-    cur_pos_motor1 = motor1.getPosition();
-    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (360*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (360*2.0*PI/360);
+    if(setpoint1 > (360*2.3*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (360*2.3*2.0*PI/360);
     }
-    if(setpoint1 < -(360*2.0*PI/360)) {
-        setpoint1 = -(360*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.0*PI/360)) {
-        staat = 1;
+    if(setpoint1 > (360*2.3*2.0*PI/360)) {
+        staat1 = 1;
     }
-    wait(TSAMP);
+    
 
 
 }
@@ -134,7 +132,7 @@
     pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
     if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
-     staat = 1;
+     staat1 = 1;
     }
     
 
@@ -227,15 +225,15 @@
         }
         wait(TSAMP);
         //STATES
-        if(staat == 0) {
+        if(staat1 == 0) {
             arm_hoog();
             wait_iterator = 0;
         } 
-        else if(staat ==1)
+        else if(staat1 ==1)
         {
             wait_iterator++;
             if(wait_iterator > 200)
-                staat = 2;
+                staat1 = 2;
         }
         else
         {