Diff: main.cpp

Revision:

12:146ba6f95fe0

Parent:

11:b2dec8f3e75c

Child:

13:4f4cc1a5a79a

--- a/main.cpp	Tue Oct 27 11:42:19 2015 +0000
+++ b/main.cpp	Tue Oct 27 11:56:57 2015 +0000
@@ -37,7 +37,7 @@
 }
 
 //define states
-enum state {HOME, MOVE_MOTOR_1, MOVE_MOTOR_2, BACKTOHOMEPOSITION, STOP};
+enum state {HOME, MOVE_MOTORS, BACKTOHOMEPOSITION, STOP};
 uint8_t state = HOME;
 
 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
@@ -153,7 +153,7 @@
 // script voor filters
 void Filters()
 {
-    // Biceps right 
+    // Biceps right
     A = EMG_bicepsright.read();
     //Highpass
     y1 = biquad (A, ah1_v1, ah1_v2, fh1_a1, fh1_a2, fh1_b0*0.753507, fh1_b1*0.753507, fh1_b2*0.753507);
@@ -241,7 +241,7 @@
         pc.printf("motor1 cw \n\r");
         motor1_direction = 0; //counterclockwise
         motor1_speed = 0.2;
-    } else if (final_filter2 > 0.04 || button_2 == pressed){
+    } else if (final_filter2 > 0.04 || button_2 == pressed) {
         pc.printf("motor1 ccw \n\r");
         motor1_direction = 1; //clockwise
         motor1_speed = 0.2;
@@ -315,71 +315,61 @@
                 H2 = motor2.getPosition();
                 pc.printf("Home-position is %f\n\r", H1);
                 pc.printf("Home-pso is %f\n\r", H2);
-                state = MOVE_MOTOR_1;
+                state = MOVE_MOTORS;
                 wait(2);
                 break;
             }
 
-            case MOVE_MOTOR_1: {          //motor kan cw en ccw bewegen a.d.h.v. buttons
+            case MOVE_MOTORS: {          //motor kan cw en ccw bewegen a.d.h.v. buttons
                 pc.printf("move_motor\n\r");
-                while (state == MOVE_MOTOR_1) {
+                while (state == MOVE_MOTORS) {
                     move_motor1();
+                    move_motor2();
                     if (button_1 == pressed && button_2 == pressed) {
                         motor1_speed = 0;
-                        state = MOVE_MOTOR_2;
-                    }
-                }
-                wait (1);
-                break;
-            }
-
-            case MOVE_MOTOR_2: {          //motor kan cw en ccw bewegen a.d.h.v. buttons
-                pc.printf("move_motor\n\r");
-                while (state == MOVE_MOTOR_2) {
-                    move_motor2();
-                    if (button_1 == pressed && button_2 == pressed) {
                         motor2_speed = 0;
                         state = BACKTOHOMEPOSITION;
                     }
                 }
                 wait (1);
                 break;
+            }
 
-                case BACKTOHOMEPOSITION: {  //motor gaat terug naar homeposition
-                    pc.printf("backhomeposition\n\r");
-                    wait (1);
 
-                    ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
-                    while(state == BACKTOHOMEPOSITION) {
-                        movetohome();
-                        while(regelaar_ticker_flag != true);
-                        regelaar_ticker_flag = false;
+            case BACKTOHOMEPOSITION: {  //motor gaat terug naar homeposition
+                pc.printf("backhomeposition\n\r");
+                wait (1);
 
-                        pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
-                        pc.printf("referentie %f, %f \n\r", H1, H2);
+                ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
+                while(state == BACKTOHOMEPOSITION) {
+                    movetohome();
+                    while(regelaar_ticker_flag != true);
+                    regelaar_ticker_flag = false;
 
-                        if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
-                            pc.printf("motor1 pos %d", motor1.getPosition());
-                            pc.printf("motor2 pos %d", motor2.getPosition());
-                            pc.printf("referentie %f %f\n\r", H1, H2);
-                            state = STOP;
-                            pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
-                            break;
-                        }
+                    pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
+                    pc.printf("referentie %f, %f \n\r", H1, H2);
+
+                    if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
+                        pc.printf("motor1 pos %d", motor1.getPosition());
+                        pc.printf("motor2 pos %d", motor2.getPosition());
+                        pc.printf("referentie %f %f\n\r", H1, H2);
+                        state = STOP;
+                        pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
+                        break;
                     }
                 }
-                case STOP: {
-                    static int c;
-                    while(state == STOP) {
-                        motor1_speed = 0;
-                        motor2_speed = 0;
-                        if (c++ == 0) {
-                            pc.printf("einde\n\r");
-                        }
+            }
+            case STOP: {
+                static int c;
+                while(state == STOP) {
+                    motor1_speed = 0;
+                    motor2_speed = 0;
+                    if (c++ == 0) {
+                        pc.printf("einde\n\r");
                     }
-                    break;
                 }
+                break;
             }
         }
     }
-}
\ No newline at end of file
+}