Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@12:88cbc65f2563, 2019-10-07 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 07 13:17:35 2019 +0000
Revision:
12:88cbc65f2563
Parent:
11:f83e3bf7febf
Child:
14:4cf17b10e504
added:; full state machine structure; EMG_params struct; comments; ; Removed:; Old code, commented out parts

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 12:88cbc65f2563 6 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 12:88cbc65f2563 7 DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 12:88cbc65f2563 8 FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 12:88cbc65f2563 9 DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 12:88cbc65f2563 10 FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 11 Ticker loop_ticker; //used in main()
joostbonekamp 12:88cbc65f2563 12 AnalogIn Pot1(A1); //pot 1 on biorobotics shield
joostbonekamp 12:88cbc65f2563 13 AnalogIn Pot2(A0); //pot 2 on biorobotics shield
joostbonekamp 12:88cbc65f2563 14 InterruptIn but1(D10); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 15 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 16
PatrickZieverink 8:6f6a4dc12036 17 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 10:b8c60fd468f1 18 QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 10:b8c60fd468f1 19
joostbonekamp 3:e3d12393adb1 20
joostbonekamp 5:aa8b5d5e632f 21 //variables
joostbonekamp 12:88cbc65f2563 22 enum States {idle, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, failure};
joostbonekamp 12:88cbc65f2563 23 States state; //using the States enum
joostbonekamp 12:88cbc65f2563 24 struct actuator_state
joostbonekamp 12:88cbc65f2563 25 {
joostbonekamp 12:88cbc65f2563 26 float duty_cycle1; //pwm of 1st motor
joostbonekamp 12:88cbc65f2563 27 float duty_cycle2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 28 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 29 int dir2; //direction of 2nd motor
joostbonekamp 12:88cbc65f2563 30 bool magnet; //state of the magnet
joostbonekamp 12:88cbc65f2563 31 } actuators;
joostbonekamp 12:88cbc65f2563 32
joostbonekamp 12:88cbc65f2563 33 struct EMG_params
joostbonekamp 12:88cbc65f2563 34 {
joostbonekamp 12:88cbc65f2563 35 float idk; //params of the emg, tbd during calibration
joostbonekamp 12:88cbc65f2563 36 } emg_values;
joostbonekamp 5:aa8b5d5e632f 37
joostbonekamp 12:88cbc65f2563 38 //initializing enums and classes
joostbonekamp 12:88cbc65f2563 39
joostbonekamp 12:88cbc65f2563 40
joostbonekamp 12:88cbc65f2563 41 int enc_zero; //the zero position of the encoders, to be determined from the
joostbonekamp 12:88cbc65f2563 42 //encoder calibration
joostbonekamp 12:88cbc65f2563 43
joostbonekamp 12:88cbc65f2563 44 //variables used throughout the program
joostbonekamp 12:88cbc65f2563 45 bool state_changed = false; //used to see if the state is "starting"
PatrickZieverink 8:6f6a4dc12036 46 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 47 volatile bool but2_pressed = false;
joostbonekamp 12:88cbc65f2563 48 float pot_1; //used to keep track of the potentiometer values
joostbonekamp 10:b8c60fd468f1 49 float pot_2;
joostbonekamp 12:88cbc65f2563 50
joostbonekamp 12:88cbc65f2563 51 void do_nothing()
joostbonekamp 5:aa8b5d5e632f 52
PatrickZieverink 9:6537eead1241 53 /*
joostbonekamp 12:88cbc65f2563 54 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 55 */
joostbonekamp 12:88cbc65f2563 56 {}
joostbonekamp 12:88cbc65f2563 57
joostbonekamp 12:88cbc65f2563 58 void failure()
joostbonekamp 12:88cbc65f2563 59 /*
joostbonekamp 12:88cbc65f2563 60 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 61 */
joostbonekamp 12:88cbc65f2563 62 {
joostbonekamp 12:88cbc65f2563 63 if (state_changed) {
joostbonekamp 12:88cbc65f2563 64 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 65 state_changed = false;
joostbonekamp 12:88cbc65f2563 66 }
joostbonekamp 12:88cbc65f2563 67 }
PatrickZieverink 9:6537eead1241 68
joostbonekamp 12:88cbc65f2563 69 void cali_EMG()
joostbonekamp 12:88cbc65f2563 70 /*
joostbonekamp 12:88cbc65f2563 71 Calibratioin of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 72 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 73 */
joostbonekamp 12:88cbc65f2563 74 {
joostbonekamp 12:88cbc65f2563 75 if (state_changed) {
joostbonekamp 12:88cbc65f2563 76 pc.printf("Started EMG calibration\r\n");
joostbonekamp 12:88cbc65f2563 77 state_changed = false;
PatrickZieverink 9:6537eead1241 78 }
joostbonekamp 12:88cbc65f2563 79 }
joostbonekamp 12:88cbc65f2563 80 void cali_enc()
joostbonekamp 12:88cbc65f2563 81 /*
joostbonekamp 12:88cbc65f2563 82 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 12:88cbc65f2563 83 position for the linear stage and the most upright postition for the
joostbonekamp 12:88cbc65f2563 84 rotating stage.
joostbonekamp 12:88cbc65f2563 85 */
joostbonekamp 12:88cbc65f2563 86 {
joostbonekamp 12:88cbc65f2563 87 if (state_changed) {
joostbonekamp 12:88cbc65f2563 88 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 89 state_changed = false;
PatrickZieverink 9:6537eead1241 90 }
joostbonekamp 12:88cbc65f2563 91 }
joostbonekamp 12:88cbc65f2563 92 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 93 /*
joostbonekamp 12:88cbc65f2563 94 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 95 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 96 */
joostbonekamp 12:88cbc65f2563 97 {
joostbonekamp 12:88cbc65f2563 98 if (state_changed) {
joostbonekamp 12:88cbc65f2563 99 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 100 state_changed = false;
PatrickZieverink 9:6537eead1241 101 }
joostbonekamp 12:88cbc65f2563 102 return;
PatrickZieverink 9:6537eead1241 103 }
joostbonekamp 12:88cbc65f2563 104 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 105 /*
joostbonekamp 12:88cbc65f2563 106 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 107 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 108 */
joostbonekamp 12:88cbc65f2563 109 {
joostbonekamp 12:88cbc65f2563 110 if (state_changed) {
joostbonekamp 12:88cbc65f2563 111 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 112 state_changed = false;
joostbonekamp 12:88cbc65f2563 113 }
joostbonekamp 12:88cbc65f2563 114 return;
PatrickZieverink 9:6537eead1241 115 }
joostbonekamp 12:88cbc65f2563 116 void homing()
joostbonekamp 12:88cbc65f2563 117 /*
joostbonekamp 12:88cbc65f2563 118 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 119 */
joostbonekamp 12:88cbc65f2563 120 {
joostbonekamp 12:88cbc65f2563 121 if (state_changed) {
joostbonekamp 12:88cbc65f2563 122 pc.printf("Started homing");
joostbonekamp 12:88cbc65f2563 123 state_changed = false;
joostbonekamp 12:88cbc65f2563 124 }
joostbonekamp 12:88cbc65f2563 125 return;
joostbonekamp 12:88cbc65f2563 126 }
PatrickZieverink 9:6537eead1241 127
PatrickZieverink 8:6f6a4dc12036 128 // the funtions needed to run the program
joostbonekamp 12:88cbc65f2563 129 void measure_signals()
joostbonekamp 12:88cbc65f2563 130 {
joostbonekamp 12:88cbc65f2563 131 return;
joostbonekamp 12:88cbc65f2563 132 }
joostbonekamp 10:b8c60fd468f1 133
joostbonekamp 12:88cbc65f2563 134 void do_nothing()
joostbonekamp 12:88cbc65f2563 135 {
joostbonekamp 12:88cbc65f2563 136 actuator.duty_cycle1 = 0;
joostbonekamp 12:88cbc65f2563 137 actuator.duty_cycle2 = 0;
joostbonekamp 11:f83e3bf7febf 138
joostbonekamp 5:aa8b5d5e632f 139
joostbonekamp 10:b8c60fd468f1 140 //state guard
joostbonekamp 10:b8c60fd468f1 141 if (but1_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 142 current_state = cw;
joostbonekamp 6:354a6509405f 143 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 144 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 145 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 146 }
joostbonekamp 12:88cbc65f2563 147
joostbonekamp 5:aa8b5d5e632f 148 }
PatrickZieverink 7:78bc59c7753c 149
joostbonekamp 12:88cbc65f2563 150 void motor_controller()
joostbonekamp 12:88cbc65f2563 151 {
joostbonekamp 12:88cbc65f2563 152
joostbonekamp 12:88cbc65f2563 153 motor1_direction = actuator.dir1; // Zet de richting goed
joostbonekamp 12:88cbc65f2563 154 motor1_pwm.write(actuator.duty_cycle1); // Zet het op de snelheid van actuator.speed1
joostbonekamp 12:88cbc65f2563 155
PatrickZieverink 7:78bc59c7753c 156
joostbonekamp 12:88cbc65f2563 157 motor2_direction = actuator.dir2;
joostbonekamp 12:88cbc65f2563 158 motor2_pwm.write(actuator.duty_cycle2);
joostbonekamp 12:88cbc65f2563 159 return;
joostbonekamp 12:88cbc65f2563 160 }
joostbonekamp 12:88cbc65f2563 161
joostbonekamp 12:88cbc65f2563 162 void output()
joostbonekamp 12:88cbc65f2563 163 {
joostbonekamp 12:88cbc65f2563 164 return;
joostbonekamp 5:aa8b5d5e632f 165 }
joostbonekamp 12:88cbc65f2563 166
joostbonekamp 12:88cbc65f2563 167 void state_machine()
joostbonekamp 12:88cbc65f2563 168 {
joostbonekamp 12:88cbc65f2563 169 //run current state
joostbonekamp 12:88cbc65f2563 170 switch (current_state) {
joostbonekamp 12:88cbc65f2563 171 case idle:
joostbonekamp 12:88cbc65f2563 172 do_nothing();
joostbonekamp 12:88cbc65f2563 173 break;
joostbonekamp 12:88cbc65f2563 174 case failure:
joostbonekamp 12:88cbc65f2563 175 failure();
joostbonekamp 12:88cbc65f2563 176 break;
joostbonekamp 12:88cbc65f2563 177 case cali_EMG:
joostbonekamp 12:88cbc65f2563 178 cali_EMG();
joostbonekamp 12:88cbc65f2563 179 break;
joostbonekamp 12:88cbc65f2563 180 case cali_ENC:
joostbonekamp 12:88cbc65f2563 181 cali_encoder();
joostbonekamp 12:88cbc65f2563 182 break;
joostbonekamp 12:88cbc65f2563 183 case moving_magnet_on:
joostbonekamp 12:88cbc65f2563 184 moving_magnet_on();
joostbonekamp 12:88cbc65f2563 185 break;
joostbonekamp 12:88cbc65f2563 186 case moving_magnet_off:
joostbonekamp 12:88cbc65f2563 187 moving_magnet_off();
joostbonekamp 12:88cbc65f2563 188 break;
joostbonekamp 12:88cbc65f2563 189 case homing:
joostbonekamp 12:88cbc65f2563 190 homing();
joostbonekamp 12:88cbc65f2563 191 break;
joostbonekamp 5:aa8b5d5e632f 192 }
joostbonekamp 5:aa8b5d5e632f 193 }
joostbonekamp 10:b8c60fd468f1 194
joostbonekamp 12:88cbc65f2563 195 void main_loop()
joostbonekamp 12:88cbc65f2563 196 {
joostbonekamp 5:aa8b5d5e632f 197 measure_signals();
joostbonekamp 5:aa8b5d5e632f 198 state_machine();
joostbonekamp 5:aa8b5d5e632f 199 motor_controller();
joostbonekamp 5:aa8b5d5e632f 200 output();
joostbonekamp 5:aa8b5d5e632f 201 }
joostbonekamp 3:e3d12393adb1 202
joostbonekamp 12:88cbc65f2563 203 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 12:88cbc65f2563 204 //state machines
joostbonekamp 12:88cbc65f2563 205 void but1_interrupt ()
joostbonekamp 12:88cbc65f2563 206 {
joostbonekamp 12:88cbc65f2563 207 but1_pressed = true;
joostbonekamp 12:88cbc65f2563 208 pc.printf("Button 1 pressed \n\r");
joostbonekamp 12:88cbc65f2563 209 }
joostbonekamp 12:88cbc65f2563 210
joostbonekamp 12:88cbc65f2563 211 void but2_interrupt ()
joostbonekamp 12:88cbc65f2563 212 {
joostbonekamp 12:88cbc65f2563 213 but2_pressed = true;
joostbonekamp 12:88cbc65f2563 214 pc.printf("Button 2 pressed \n\r");
joostbonekamp 12:88cbc65f2563 215 }
joostbonekamp 12:88cbc65f2563 216
joostbonekamp 12:88cbc65f2563 217 int main()
joostbonekamp 12:88cbc65f2563 218 {
joostbonekamp 3:e3d12393adb1 219 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 220 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 221 current_state = idle;
joostbonekamp 3:e3d12393adb1 222
joostbonekamp 12:88cbc65f2563 223 motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait
joostbonekamp 12:88cbc65f2563 224 motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait
joostbonekamp 12:88cbc65f2563 225
joostbonekamp 12:88cbc65f2563 226 actuator.dir1 = 0;
joostbonekamp 12:88cbc65f2563 227 actuator.dir2 = 0;
joostbonekamp 12:88cbc65f2563 228
joostbonekamp 12:88cbc65f2563 229 actuator.magnet = false;
PatrickZieverink 7:78bc59c7753c 230
PatrickZieverink 8:6f6a4dc12036 231 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 232 but2.fall(&but2_interrupt);
joostbonekamp 12:88cbc65f2563 233 loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
joostbonekamp 12:88cbc65f2563 234 pc.printf("Main_loop is running\n\r");
joostbonekamp 5:aa8b5d5e632f 235 while (true) {}
joostbonekamp 12:88cbc65f2563 236 }