![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Fork of statemachineopzet by
Diff: main.cpp
- Revision:
- 2:afa78538ef33
- Parent:
- 1:1541f49d8680
diff -r 1541f49d8680 -r afa78538ef33 main.cpp --- a/main.cpp Fri Nov 02 11:44:30 2018 +0000 +++ b/main.cpp Fri Nov 02 14:01:29 2018 +0000 @@ -593,22 +593,31 @@ case CHOOSE: //State to choose between RKI and DC if(StateChanged) { - //pc.printf("Entering Basic Control Mode\r\n"); ledr = 1; ledg = 1; ledb = 0; StateChanged = false; } + + while(button1 && button3 && button4) + {;} + if(!button3) - { - CurrentState = RKI; - StateChanged = true; - } + { + CurrentState = RKI; + StateChanged = true; + } else if(!button4) - { - CurrentState = DC; - StateChanged = true; - } + { + CurrentState = DC; + StateChanged = true; + } + else + { + CurrentState = WAITING; + StateChanged = true; + } + break; //End of "choose" state, move on to RKI or DC (straight lines or direct control) case RKI: //State for moving the robot in straight lines in cartesian space @@ -617,7 +626,7 @@ { //pc.printf("Entering Basic Control Mode\r\n"); ledr = 1; - ledg = 1; + ledg = 0; ledb = 0; StateChanged = false; } @@ -658,12 +667,12 @@ start_v_des_calculate_qref = 0; StateChanged = true; } - break; //End of BCM state + break; //End of RKI state case DC: if(StateChanged) { - ledr = 1; + ledr = 0; ledg = 1; ledb = 0; StateChanged = false; @@ -706,8 +715,8 @@ case HOMING: //Back to encoder position zero if(StateChanged) { - ledr = 1; - ledg = 1; + ledr = 0; + ledg = 0; ledb = 0; StateChanged = false; } @@ -715,7 +724,38 @@ while(button1 && !failure &&!backRKI &&!backDC) { //Back to encoder position zero function + + //translation back + while(encoder1.getPulses()>10.0 && encoder1.getPulses()<-10.0) + { + + if(encoder1.getPulses()>10.0) + { + pwmpin1 = 0.3f; + directionpin1.write(0); + } + else if (encoder1.getPulses()<-10.0) + { + pwmpin1 = 0.3f; + directionpin1.write(1); + } + } + //Rotation back + while(encoder2.getPulses()>10.0 && encoder2.getPulses()<-10.0) + { + + if(encoder2.getPulses()>10.0) + { + pmwpin2 = 0.3f; + directionpin2.write(1); + } + else if (encoder2.getPulses()<-10.0) + { + pwmpin2 = 0.3f; + directionpin2.write(0); + } + } if(!button3) @@ -762,12 +802,18 @@ { failure = false; StateChanged = false; + + ledr=0; + ledg=0; + ledb=0; } //State actions //"Fail mode is active. Press button1 to enter WAITING - while(button1) - { - } + while(button1) + { + ledr = !ledr; + wait(0.3f); + } //State transition CurrentState = WAITING; StateChanged = true; @@ -780,7 +826,7 @@ ledb = 0; while(button1) { - ledr = !ledr; + ledb = !ledb; wait(0.3f); } CurrentState = WAITING;