Diff: main.cpp

Revision:

26:5981d4b1144e

Parent:

25:f45d61cc6dc6

Child:

27:77bb255753be

Child:

28:4f2e15118f13

--- a/main.cpp	Fri Oct 31 11:27:31 2014 +0000
+++ b/main.cpp	Fri Oct 31 12:06:01 2014 +0000
@@ -118,8 +118,7 @@
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
 {
     speed1_rad = 6.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 > (155.0*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155.0*2.0*PI/360);
@@ -129,29 +128,30 @@
     }
     pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
-   //if(setpoint1 = (155.0*2.0*PI/360)) {
-     // staat = 1;
-    //}
-    wait(TSAMP);
+    if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
+     staat = 1;
+    }
+    
 
 }
 
 void arm_mid ()
 {
     speed1_rad = -3.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;
+    //cur_pos_motor1 = motor1.getPosition();
+    //os_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
     if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155*2.0*PI/360);
     }
     if(setpoint1 < -(155*2.0*PI/360)) {
         setpoint1 = -(155*2.0*PI/360);
     }
-    pwm_motor1.write(abs(pwm1_percentage));
+    //pc.printf("m%f\n\r",setpoint1);
+    //pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
     
-    wait(TSAMP);
+    //wait(TSAMP);
 }
 
 void arm_laag ()
@@ -174,6 +174,7 @@
 
 int main()
 {
+    int wait_iterator = 0;
     pwm_motor1.period_us(100);
     motor1.setPosition(0);
     pwm_motor2.period_us(100);
@@ -181,7 +182,10 @@
     pc.baud(115200);
 
     while(1) {
+        //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
         pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
         if (pwm1_percentage < -1.0) {
             pwm1_percentage = -1.0;
@@ -189,18 +193,25 @@
         if (pwm1_percentage > 1.0) {
             pwm1_percentage = 1.0;
         }
-
+        pwm_motor1.write(abs(pwm1_percentage));
         if(pwm1_percentage > 0) {
             motordir1 = 0;
         } else {
             motordir1 = 1;
         }
-
-
+        wait(TSAMP);
+        //STATES
         if(staat == 0) {
             arm_hoog();
+            wait_iterator = 0;
         } 
-        else 
+        else if(staat ==1)
+        {
+            wait_iterator++;
+            if(wait_iterator > 200)
+                staat = 2;
+        }
+        else
         {
             arm_mid();
         }