Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_Patrick

Werkt dit?

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@5:aa8b5d5e632f, 2019-10-03 (annotated)

Committer:
joostbonekamp
Date:
Thu Oct 03 10:53:17 2019 +0000
Revision:
5:aa8b5d5e632f
Parent:
4:36e32ddf2443
Child:
6:354a6509405f
state machine first attempt;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 3 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 4 #include "QEI.h"
RobertoO 0:67c50348f842 5
joostbonekamp 4:36e32ddf2443 6 MODSERIAL pc(USBTX, USBRX); //verbinden met pc
joostbonekamp 4:36e32ddf2443 7 DigitalOut motor2_direction(D4); //verbinden met motor 2 op board (altijd d4)
joostbonekamp 5:aa8b5d5e632f 8 FastPWM motor2_pwm(D5); //verbinden met motor 2 pwm (altijd d5)
joostbonekamp 5:aa8b5d5e632f 9 Ticker loop_ticker; //used in main()
joostbonekamp 5:aa8b5d5e632f 10 InterruptIn button(D10); //knop op birorobotics shield
joostbonekamp 4:36e32ddf2443 11 QEI encoder (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder gebruiken
joostbonekamp 3:e3d12393adb1 12
joostbonekamp 5:aa8b5d5e632f 13 //variables
joostbonekamp 5:aa8b5d5e632f 14 enum States {idle, cw, ccw, end, failure};
joostbonekamp 5:aa8b5d5e632f 15 States current_state;
joostbonekamp 5:aa8b5d5e632f 16 class motor_state {
joostbonekamp 5:aa8b5d5e632f 17 public:
joostbonekamp 5:aa8b5d5e632f 18 float pwm1; //pwm of 1st motor
joostbonekamp 5:aa8b5d5e632f 19 float pwm2; //pwm of 2nd motor
joostbonekamp 5:aa8b5d5e632f 20 int dir1; //direction of 1st motor
joostbonekamp 5:aa8b5d5e632f 21 int dir2; //direction of 2nd motor
joostbonekamp 5:aa8b5d5e632f 22 };
joostbonekamp 5:aa8b5d5e632f 23 motor_state motor;
joostbonekamp 5:aa8b5d5e632f 24
joostbonekamp 5:aa8b5d5e632f 25 bool state_changed = false;
joostbonekamp 5:aa8b5d5e632f 26 volatile bool button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 27
joostbonekamp 5:aa8b5d5e632f 28 void measure_signals() {return;}
joostbonekamp 5:aa8b5d5e632f 29
joostbonekamp 5:aa8b5d5e632f 30 void do_nothing() {
joostbonekamp 5:aa8b5d5e632f 31 if (button_pressed) {
joostbonekamp 5:aa8b5d5e632f 32 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 33 state_changed = true;
joostbonekamp 5:aa8b5d5e632f 34 pc.printf("Changed state from idle to cw\r\n");
joostbonekamp 5:aa8b5d5e632f 35 button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 36 }
joostbonekamp 5:aa8b5d5e632f 37 }
joostbonekamp 5:aa8b5d5e632f 38 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 39 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 40 pc.printf("State changed to CW\r\n");
joostbonekamp 5:aa8b5d5e632f 41 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 42 }
joostbonekamp 5:aa8b5d5e632f 43 motor.dir2 = 1;
joostbonekamp 5:aa8b5d5e632f 44 motor.pwm2 = 0.5f;
joostbonekamp 5:aa8b5d5e632f 45 if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 46 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 47 state_changed = true;
joostbonekamp 5:aa8b5d5e632f 48 button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 49 }
joostbonekamp 5:aa8b5d5e632f 50 }
joostbonekamp 5:aa8b5d5e632f 51
joostbonekamp 5:aa8b5d5e632f 52 void rotate_ccw() {
joostbonekamp 5:aa8b5d5e632f 53 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 54 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 55 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 56 }
joostbonekamp 5:aa8b5d5e632f 57 motor.dir2 = 0;
joostbonekamp 5:aa8b5d5e632f 58 motor.pwm2 = 0.5f;
joostbonekamp 5:aa8b5d5e632f 59 if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 60 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 61 state_changed = true;
joostbonekamp 5:aa8b5d5e632f 62 button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 63 }
joostbonekamp 5:aa8b5d5e632f 64 }
joostbonekamp 5:aa8b5d5e632f 65
joostbonekamp 5:aa8b5d5e632f 66 void motor_controller() {return;}
joostbonekamp 5:aa8b5d5e632f 67 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 68
joostbonekamp 5:aa8b5d5e632f 69 void button_interrupt () {
joostbonekamp 5:aa8b5d5e632f 70 button_pressed = true;
joostbonekamp 5:aa8b5d5e632f 71 }
joostbonekamp 5:aa8b5d5e632f 72
joostbonekamp 5:aa8b5d5e632f 73 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 74
joostbonekamp 5:aa8b5d5e632f 75 //run current state
joostbonekamp 5:aa8b5d5e632f 76 switch (current_state) {
joostbonekamp 5:aa8b5d5e632f 77 case idle:
joostbonekamp 5:aa8b5d5e632f 78 do_nothing();
joostbonekamp 5:aa8b5d5e632f 79 break;
joostbonekamp 5:aa8b5d5e632f 80 case cw:
joostbonekamp 5:aa8b5d5e632f 81 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 82 break;
joostbonekamp 5:aa8b5d5e632f 83 case ccw:
joostbonekamp 5:aa8b5d5e632f 84 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 85 break;
joostbonekamp 5:aa8b5d5e632f 86 case end:
joostbonekamp 5:aa8b5d5e632f 87 break;
joostbonekamp 5:aa8b5d5e632f 88 case failure:
joostbonekamp 5:aa8b5d5e632f 89 break;
joostbonekamp 5:aa8b5d5e632f 90 }
joostbonekamp 5:aa8b5d5e632f 91 }
joostbonekamp 5:aa8b5d5e632f 92
joostbonekamp 5:aa8b5d5e632f 93 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 94 measure_signals();
joostbonekamp 5:aa8b5d5e632f 95 state_machine();
joostbonekamp 5:aa8b5d5e632f 96 motor_controller();
joostbonekamp 5:aa8b5d5e632f 97 output();
joostbonekamp 5:aa8b5d5e632f 98 }
joostbonekamp 3:e3d12393adb1 99
joostbonekamp 2:bbaa6fca2ab1 100 int main() {
joostbonekamp 3:e3d12393adb1 101 pc.baud(115200);
joostbonekamp 5:aa8b5d5e632f 102 pc.printf("Executing main()\r\n");
joostbonekamp 5:aa8b5d5e632f 103 current_state = idle;
joostbonekamp 3:e3d12393adb1 104
joostbonekamp 5:aa8b5d5e632f 105 motor.pwm1 = 0;
joostbonekamp 5:aa8b5d5e632f 106 motor.pwm2 = 0;
joostbonekamp 5:aa8b5d5e632f 107 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 108 motor.dir2 = 0;
joostbonekamp 5:aa8b5d5e632f 109 button.fall(&button_interrupt);
joostbonekamp 5:aa8b5d5e632f 110 loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
joostbonekamp 5:aa8b5d5e632f 111
joostbonekamp 5:aa8b5d5e632f 112 while (true) {}
joostbonekamp 5:aa8b5d5e632f 113 //while (true) {
joostbonekamp 5:aa8b5d5e632f 114 // enc_count = encoder.getPulses(); //kijkt op welke stand hij nu staat
joostbonekamp 5:aa8b5d5e632f 115 // pc.printf("%f\r\n", float(enc_count-prev_count)*360/(8400*0.025)); //verschil met vorige stap in graden per stap gedeeld door tijd = snelheid
joostbonekamp 5:aa8b5d5e632f 116 // prev_count = enc_count;
joostbonekamp 5:aa8b5d5e632f 117 // wait_ms(25);
joostbonekamp 5:aa8b5d5e632f 118 //}
RobertoO 0:67c50348f842 119 }
joostbonekamp 2:bbaa6fca2ab1 120