Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 12:146ba6f95fe0
- Parent:
- 11:b2dec8f3e75c
- Child:
- 13:4f4cc1a5a79a
diff -r b2dec8f3e75c -r 146ba6f95fe0 main.cpp --- 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 +}