Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:354a6509405f, 2019-10-03 (annotated)

Committer:
joostbonekamp
Date:
Thu Oct 03 11:02:02 2019 +0000
Revision:
6:354a6509405f
Parent:
5:aa8b5d5e632f
Child:
7:78bc59c7753c
added comments;

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 6:354a6509405f 31 if (button_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 32 current_state = cw;
joostbonekamp 6:354a6509405f 33 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 34 pc.printf("Changed state from idle to cw\r\n");
joostbonekamp 6:354a6509405f 35 button_pressed = false; //reset button
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 6:354a6509405f 41 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 42 }
joostbonekamp 6:354a6509405f 43 motor.dir2 = 1; //todo: check if this is actually clockwise
joostbonekamp 5:aa8b5d5e632f 44 motor.pwm2 = 0.5f;
joostbonekamp 6:354a6509405f 45 if (!state_changed && button_pressed) { //state wasnt just changed, button has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 46 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 47 state_changed = true;
joostbonekamp 6:354a6509405f 48 button_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 49 }
joostbonekamp 5:aa8b5d5e632f 50 }
joostbonekamp 5:aa8b5d5e632f 51
joostbonekamp 5:aa8b5d5e632f 52 void rotate_ccw() {
joostbonekamp 6:354a6509405f 53 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 54 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 55 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 56 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 57 }
joostbonekamp 5:aa8b5d5e632f 58 motor.dir2 = 0;
joostbonekamp 5:aa8b5d5e632f 59 motor.pwm2 = 0.5f;
joostbonekamp 5:aa8b5d5e632f 60 if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 61 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 62 state_changed = true;
joostbonekamp 5:aa8b5d5e632f 63 button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 64 }
joostbonekamp 5:aa8b5d5e632f 65 }
joostbonekamp 5:aa8b5d5e632f 66
joostbonekamp 5:aa8b5d5e632f 67 void motor_controller() {return;}
joostbonekamp 5:aa8b5d5e632f 68 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 69
joostbonekamp 5:aa8b5d5e632f 70 void button_interrupt () {
joostbonekamp 5:aa8b5d5e632f 71 button_pressed = true;
joostbonekamp 5:aa8b5d5e632f 72 }
joostbonekamp 5:aa8b5d5e632f 73
joostbonekamp 5:aa8b5d5e632f 74 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 75
joostbonekamp 5:aa8b5d5e632f 76 //run current state
joostbonekamp 5:aa8b5d5e632f 77 switch (current_state) {
joostbonekamp 5:aa8b5d5e632f 78 case idle:
joostbonekamp 5:aa8b5d5e632f 79 do_nothing();
joostbonekamp 5:aa8b5d5e632f 80 break;
joostbonekamp 5:aa8b5d5e632f 81 case cw:
joostbonekamp 5:aa8b5d5e632f 82 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 83 break;
joostbonekamp 5:aa8b5d5e632f 84 case ccw:
joostbonekamp 5:aa8b5d5e632f 85 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 86 break;
joostbonekamp 5:aa8b5d5e632f 87 case end:
joostbonekamp 5:aa8b5d5e632f 88 break;
joostbonekamp 5:aa8b5d5e632f 89 case failure:
joostbonekamp 5:aa8b5d5e632f 90 break;
joostbonekamp 5:aa8b5d5e632f 91 }
joostbonekamp 5:aa8b5d5e632f 92 }
joostbonekamp 5:aa8b5d5e632f 93
joostbonekamp 5:aa8b5d5e632f 94 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 95 measure_signals();
joostbonekamp 5:aa8b5d5e632f 96 state_machine();
joostbonekamp 5:aa8b5d5e632f 97 motor_controller();
joostbonekamp 5:aa8b5d5e632f 98 output();
joostbonekamp 5:aa8b5d5e632f 99 }
joostbonekamp 3:e3d12393adb1 100
joostbonekamp 2:bbaa6fca2ab1 101 int main() {
joostbonekamp 3:e3d12393adb1 102 pc.baud(115200);
joostbonekamp 5:aa8b5d5e632f 103 pc.printf("Executing main()\r\n");
joostbonekamp 5:aa8b5d5e632f 104 current_state = idle;
joostbonekamp 3:e3d12393adb1 105
joostbonekamp 5:aa8b5d5e632f 106 motor.pwm1 = 0;
joostbonekamp 5:aa8b5d5e632f 107 motor.pwm2 = 0;
joostbonekamp 5:aa8b5d5e632f 108 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 109 motor.dir2 = 0;
joostbonekamp 5:aa8b5d5e632f 110 button.fall(&button_interrupt);
joostbonekamp 5:aa8b5d5e632f 111 loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
joostbonekamp 5:aa8b5d5e632f 112
joostbonekamp 5:aa8b5d5e632f 113 while (true) {}
joostbonekamp 6:354a6509405f 114 }