Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 4:b4530fb376dd
- Parent:
- 3:5f59cbe53d7d
- Child:
- 5:b9d5d7311dac
diff -r 5f59cbe53d7d -r b4530fb376dd main.cpp --- a/main.cpp Tue Oct 20 13:15:35 2015 +0000 +++ b/main.cpp Wed Oct 21 11:50:15 2015 +0000 @@ -26,8 +26,8 @@ #define SAMPLETIME_REGELAAR 0.005 //define states -enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; -uint8_t state = GOTOHOMEPOSITION; +enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; +uint8_t state = HOME; // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering const double g = 360; // Aantal graden 1 rotatie @@ -75,32 +75,31 @@ double D; -void sethome(){ +void gethome(){ H = motor1.getPosition(); } void move_motor1_ccw (){ motor1_direction = 0; - motor1_speed = 0.8; + motor1_speed = 1; } void move_motor1_cw (){ motor1_direction = 1; - motor1_speed = 0.8; + motor1_speed = 1; } void movetohome(){ P = motor1.getPosition(); - D = (P - H); - - if (D >= -36 && D <= 36){ + + if (P >= -28 && P <= 28){ motor1_speed = 0; } - else if (D > 24){ + else if (P > 24){ motor1_direction = 1; motor1_speed = 0.1; } - else if (D < -24){ + else if (P < -24){ motor1_direction = 0; motor1_speed = 0.1; } @@ -125,6 +124,10 @@ wait(0.2f); } +void print_position(){ + pc.printf("move motor \n\r"); + wait(0.2f); + } int main() { while (true) { @@ -132,12 +135,14 @@ switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases - case GOTOHOMEPOSITION: //positie op 0 zetten voor arm 1 + case HOME: //positie op 0 zetten voor arm 1 { - pc.printf("gotohomeposition\n\r"); + pc.printf("home\n\r"); read_encoder1(); - sethome(); + gethome(); + pc.printf("Home-position is %f\n\r", H); state = MOVE_MOTOR; + wait(5); break; } @@ -145,8 +150,8 @@ { pc.printf("move_motor\n\r"); while (state == MOVE_MOTOR){ - pc.printf("whiletrue\n\r"); move_motor1(); + print_position(); if (button_1 == pressed && button_2 == pressed){ state = BACKTOHOMEPOSITION; } @@ -168,26 +173,24 @@ regelaar_ticker_flag = false; pc.printf("pulsmotorposition1 %d", motor1.getPosition()); - pc.printf("referentie %d\n\r", H); + pc.printf("referentie %f\n\r", H); - if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){ - motor1.setPosition(0); - H = motor1.getPosition(); + if (P <=24 && P >= -24){ + pc.printf("pulsmotorposition1 %d", motor1.getPosition()); + pc.printf("referentie %f\n\r", H); state = STOP; + pc.printf("Laatste positie %f\n\r", motor1.getPosition()); break; } } } case STOP: { - const int einde = 1; + static int c; while(state == STOP){ motor1_speed = 0; - if (einde == 1){ - pc.printf("homepositie %d\n\r", H); - pc.printf("huidige positie %d\n\r", P); + if (c++ == 0){ pc.printf("einde\n\r"); - } } break;