Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@7:78bc59c7753c, 2019-10-04 (annotated)

Committer:
PatrickZieverink
Date:
Fri Oct 04 11:18:15 2019 +0000
Revision:
7:78bc59c7753c
Parent:
6:354a6509405f
Child:
8:6f6a4dc12036
Werkende knop zonder motor;

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
PatrickZieverink 7:78bc59c7753c 6 Serial 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
PatrickZieverink 7:78bc59c7753c 11 DigitalOut led1(D9); // led op D9 aanzetten
joostbonekamp 4:36e32ddf2443 12 QEI encoder (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder gebruiken
joostbonekamp 3:e3d12393adb1 13
joostbonekamp 5:aa8b5d5e632f 14 //variables
joostbonekamp 5:aa8b5d5e632f 15 enum States {idle, cw, ccw, end, failure};
PatrickZieverink 7:78bc59c7753c 16 States current_state; //wat gebeurd hier?
PatrickZieverink 7:78bc59c7753c 17 class motor_state { // je maakt hier motor_state?
PatrickZieverink 7:78bc59c7753c 18 public: //wat gebeurd hier?
joostbonekamp 5:aa8b5d5e632f 19 float pwm1; //pwm of 1st motor
joostbonekamp 5:aa8b5d5e632f 20 float pwm2; //pwm of 2nd motor
joostbonekamp 5:aa8b5d5e632f 21 int dir1; //direction of 1st motor
joostbonekamp 5:aa8b5d5e632f 22 int dir2; //direction of 2nd motor
joostbonekamp 5:aa8b5d5e632f 23 };
PatrickZieverink 7:78bc59c7753c 24 motor_state motor; //
joostbonekamp 5:aa8b5d5e632f 25
joostbonekamp 5:aa8b5d5e632f 26 bool state_changed = false;
joostbonekamp 5:aa8b5d5e632f 27 volatile bool button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 28
joostbonekamp 5:aa8b5d5e632f 29 void measure_signals() {return;}
joostbonekamp 5:aa8b5d5e632f 30
joostbonekamp 5:aa8b5d5e632f 31 void do_nothing() {
PatrickZieverink 7:78bc59c7753c 32
PatrickZieverink 7:78bc59c7753c 33
PatrickZieverink 7:78bc59c7753c 34 if (button_pressed == true) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 35 current_state = cw;
PatrickZieverink 7:78bc59c7753c 36 led1 = 0;
joostbonekamp 6:354a6509405f 37 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 38 pc.printf("Changed state from idle to cw\r\n");
joostbonekamp 6:354a6509405f 39 button_pressed = false; //reset button
joostbonekamp 5:aa8b5d5e632f 40 }
PatrickZieverink 7:78bc59c7753c 41
joostbonekamp 5:aa8b5d5e632f 42 }
PatrickZieverink 7:78bc59c7753c 43
PatrickZieverink 7:78bc59c7753c 44
joostbonekamp 5:aa8b5d5e632f 45 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 46 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 47 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 48 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 49 }
PatrickZieverink 7:78bc59c7753c 50 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 51
joostbonekamp 6:354a6509405f 52 if (!state_changed && button_pressed) { //state wasnt just changed, button has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 53 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 54 state_changed = true;
joostbonekamp 6:354a6509405f 55 button_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 56 }
joostbonekamp 5:aa8b5d5e632f 57 }
PatrickZieverink 7:78bc59c7753c 58
joostbonekamp 5:aa8b5d5e632f 59 void rotate_ccw() {
joostbonekamp 6:354a6509405f 60 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 61 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 62 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 63 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 64 }
joostbonekamp 5:aa8b5d5e632f 65 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 66
joostbonekamp 5:aa8b5d5e632f 67 if (!state_changed && button_pressed) { //state niet veranderd, button gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 68 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 69 state_changed = true;
joostbonekamp 5:aa8b5d5e632f 70 button_pressed = false;
joostbonekamp 5:aa8b5d5e632f 71 }
joostbonekamp 5:aa8b5d5e632f 72 }
joostbonekamp 5:aa8b5d5e632f 73
joostbonekamp 5:aa8b5d5e632f 74 void motor_controller() {return;}
joostbonekamp 5:aa8b5d5e632f 75 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 76
joostbonekamp 5:aa8b5d5e632f 77 void button_interrupt () {
joostbonekamp 5:aa8b5d5e632f 78 button_pressed = true;
joostbonekamp 5:aa8b5d5e632f 79 }
joostbonekamp 5:aa8b5d5e632f 80
joostbonekamp 5:aa8b5d5e632f 81 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 82
joostbonekamp 5:aa8b5d5e632f 83 //run current state
joostbonekamp 5:aa8b5d5e632f 84 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 85 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 86 do_nothing();
joostbonekamp 5:aa8b5d5e632f 87 break;
joostbonekamp 5:aa8b5d5e632f 88 case cw:
joostbonekamp 5:aa8b5d5e632f 89 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 90 break;
joostbonekamp 5:aa8b5d5e632f 91 case ccw:
joostbonekamp 5:aa8b5d5e632f 92 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 93 break;
joostbonekamp 5:aa8b5d5e632f 94 case end:
joostbonekamp 5:aa8b5d5e632f 95 break;
joostbonekamp 5:aa8b5d5e632f 96 case failure:
joostbonekamp 5:aa8b5d5e632f 97 break;
joostbonekamp 5:aa8b5d5e632f 98 }
joostbonekamp 5:aa8b5d5e632f 99 }
joostbonekamp 5:aa8b5d5e632f 100
joostbonekamp 5:aa8b5d5e632f 101 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 102 measure_signals();
joostbonekamp 5:aa8b5d5e632f 103 state_machine();
joostbonekamp 5:aa8b5d5e632f 104 motor_controller();
joostbonekamp 5:aa8b5d5e632f 105 output();
joostbonekamp 5:aa8b5d5e632f 106 }
joostbonekamp 3:e3d12393adb1 107
joostbonekamp 2:bbaa6fca2ab1 108 int main() {
joostbonekamp 3:e3d12393adb1 109 pc.baud(115200);
joostbonekamp 5:aa8b5d5e632f 110 pc.printf("Executing main()\r\n");
joostbonekamp 5:aa8b5d5e632f 111 current_state = idle;
joostbonekamp 3:e3d12393adb1 112
joostbonekamp 5:aa8b5d5e632f 113 motor.pwm1 = 0;
joostbonekamp 5:aa8b5d5e632f 114 motor.pwm2 = 0;
joostbonekamp 5:aa8b5d5e632f 115 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 116 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 117
PatrickZieverink 7:78bc59c7753c 118 //motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 119 //motor.pwm2 = 0.5f;
PatrickZieverink 7:78bc59c7753c 120
joostbonekamp 5:aa8b5d5e632f 121 button.fall(&button_interrupt);
PatrickZieverink 7:78bc59c7753c 122 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz
PatrickZieverink 7:78bc59c7753c 123 pc.printf("Iickerloop finished \n \r");
joostbonekamp 5:aa8b5d5e632f 124 while (true) {}
joostbonekamp 6:354a6509405f 125 }