Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@16:696e9cbcc823, 2019-10-10 (annotated)

Committer:
joostbonekamp
Date:
Thu Oct 10 11:33:38 2019 +0000
Revision:
16:696e9cbcc823
Parent:
15:9a1f34bc9958
Child:
17:615c5d8b3710
added schmitt trigger for speed

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"
hidde1104 13:51ae2da8da55 5 #define PI 3.14159265
RobertoO 0:67c50348f842 6
joostbonekamp 12:88cbc65f2563 7 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 12:88cbc65f2563 8 DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 12:88cbc65f2563 9 FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 12:88cbc65f2563 10 DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 12:88cbc65f2563 11 FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 12 Ticker loop_ticker; //used in main()
joostbonekamp 12:88cbc65f2563 13 AnalogIn Pot1(A1); //pot 1 on biorobotics shield
joostbonekamp 12:88cbc65f2563 14 AnalogIn Pot2(A0); //pot 2 on biorobotics shield
joostbonekamp 12:88cbc65f2563 15 InterruptIn but1(D10); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 16 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 16:696e9cbcc823 17 AnalogIn EMG1(A2);
joostbonekamp 16:696e9cbcc823 18 AnalogIn EMG2(A3);
joostbonekamp 12:88cbc65f2563 19
PatrickZieverink 8:6f6a4dc12036 20 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 10:b8c60fd468f1 21 QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 10:b8c60fd468f1 22
joostbonekamp 3:e3d12393adb1 23
joostbonekamp 5:aa8b5d5e632f 24 //variables
joostbonekamp 12:88cbc65f2563 25 enum States {idle, cali_EMG, cali_enc, moving_magnet_off, moving_magnet_on, homing, failure};
joostbonekamp 12:88cbc65f2563 26 States state; //using the States enum
joostbonekamp 14:4cf17b10e504 27 struct actuator_state {
joostbonekamp 12:88cbc65f2563 28 float duty_cycle1; //pwm of 1st motor
joostbonekamp 12:88cbc65f2563 29 float duty_cycle2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 30 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 31 int dir2; //direction of 2nd motor
joostbonekamp 12:88cbc65f2563 32 bool magnet; //state of the magnet
joostbonekamp 12:88cbc65f2563 33 } actuators;
joostbonekamp 12:88cbc65f2563 34
joostbonekamp 14:4cf17b10e504 35 struct EMG_params {
joostbonekamp 16:696e9cbcc823 36 float max; //params of the emg, tbd during calibration
joostbonekamp 16:696e9cbcc823 37 } EMG_values;
joostbonekamp 5:aa8b5d5e632f 38
joostbonekamp 16:696e9cbcc823 39 int enc1_zero; //the zero position of the encoders, to be determined from the
joostbonekamp 16:696e9cbcc823 40 int enc2_zero; //encoder calibration
joostbonekamp 16:696e9cbcc823 41 int EMG1_filtered;
joostbonekamp 16:696e9cbcc823 42 int EMG2_filtered;
joostbonekamp 16:696e9cbcc823 43 int enc1_value;
joostbonekamp 16:696e9cbcc823 44 int enc2_value;
joostbonekamp 12:88cbc65f2563 45
joostbonekamp 12:88cbc65f2563 46 //variables used throughout the program
joostbonekamp 12:88cbc65f2563 47 bool state_changed = false; //used to see if the state is "starting"
PatrickZieverink 8:6f6a4dc12036 48 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 49 volatile bool but2_pressed = false;
joostbonekamp 16:696e9cbcc823 50 volatile bool failure_occurred = false;
joostbonekamp 12:88cbc65f2563 51 float pot_1; //used to keep track of the potentiometer values
joostbonekamp 10:b8c60fd468f1 52 float pot_2;
joostbonekamp 16:696e9cbcc823 53 bool enc_has_been_calibrated;
joostbonekamp 16:696e9cbcc823 54 bool EMG_has_been_calibrated;
joostbonekamp 12:88cbc65f2563 55
joostbonekamp 12:88cbc65f2563 56 void do_nothing()
joostbonekamp 5:aa8b5d5e632f 57
PatrickZieverink 9:6537eead1241 58 /*
joostbonekamp 12:88cbc65f2563 59 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 60 */
joostbonekamp 16:696e9cbcc823 61 {
joostbonekamp 16:696e9cbcc823 62 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 63 state_changed = true;
joostbonekamp 16:696e9cbcc823 64 state = cali_enc;
joostbonekamp 16:696e9cbcc823 65 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 66 }
joostbonekamp 16:696e9cbcc823 67 }
joostbonekamp 12:88cbc65f2563 68
joostbonekamp 12:88cbc65f2563 69 void failure()
joostbonekamp 12:88cbc65f2563 70 /*
joostbonekamp 12:88cbc65f2563 71 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 72 */
joostbonekamp 12:88cbc65f2563 73 {
joostbonekamp 12:88cbc65f2563 74 if (state_changed) {
joostbonekamp 12:88cbc65f2563 75 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 76 state_changed = false;
joostbonekamp 12:88cbc65f2563 77 }
joostbonekamp 12:88cbc65f2563 78 }
PatrickZieverink 9:6537eead1241 79
joostbonekamp 12:88cbc65f2563 80 void cali_EMG()
joostbonekamp 12:88cbc65f2563 81 /*
joostbonekamp 16:696e9cbcc823 82 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 83 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 84 */
joostbonekamp 12:88cbc65f2563 85 {
joostbonekamp 12:88cbc65f2563 86 if (state_changed) {
joostbonekamp 12:88cbc65f2563 87 pc.printf("Started EMG calibration\r\n");
joostbonekamp 12:88cbc65f2563 88 state_changed = false;
PatrickZieverink 9:6537eead1241 89 }
joostbonekamp 12:88cbc65f2563 90 }
joostbonekamp 16:696e9cbcc823 91
joostbonekamp 12:88cbc65f2563 92 void cali_enc()
joostbonekamp 12:88cbc65f2563 93 /*
joostbonekamp 14:4cf17b10e504 94 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 14:4cf17b10e504 95 position for the linear stage and the most upright postition for the
joostbonekamp 12:88cbc65f2563 96 rotating stage.
joostbonekamp 12:88cbc65f2563 97 */
joostbonekamp 12:88cbc65f2563 98 {
joostbonekamp 12:88cbc65f2563 99 if (state_changed) {
joostbonekamp 12:88cbc65f2563 100 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 101 state_changed = false;
PatrickZieverink 9:6537eead1241 102 }
joostbonekamp 16:696e9cbcc823 103 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 104 enc1_zero = enc1_value;
joostbonekamp 16:696e9cbcc823 105 enc2_zero = enc2_value;
joostbonekamp 16:696e9cbcc823 106 enc_has_been_calibrated = true;
joostbonekamp 16:696e9cbcc823 107 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 108 state = moving_magnet_off;
joostbonekamp 16:696e9cbcc823 109 state_changed = true;
joostbonekamp 16:696e9cbcc823 110
joostbonekamp 16:696e9cbcc823 111 }
joostbonekamp 12:88cbc65f2563 112 }
joostbonekamp 16:696e9cbcc823 113
joostbonekamp 12:88cbc65f2563 114 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 115 /*
joostbonekamp 14:4cf17b10e504 116 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 117 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 118 */
joostbonekamp 12:88cbc65f2563 119 {
joostbonekamp 12:88cbc65f2563 120 if (state_changed) {
joostbonekamp 12:88cbc65f2563 121 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 122 state_changed = false;
PatrickZieverink 9:6537eead1241 123 }
PatrickZieverink 9:6537eead1241 124 }
joostbonekamp 16:696e9cbcc823 125
joostbonekamp 12:88cbc65f2563 126 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 127 /*
joostbonekamp 14:4cf17b10e504 128 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 129 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 130 */
joostbonekamp 12:88cbc65f2563 131 {
joostbonekamp 12:88cbc65f2563 132 if (state_changed) {
joostbonekamp 12:88cbc65f2563 133 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 134 state_changed = false;
joostbonekamp 12:88cbc65f2563 135 }
joostbonekamp 12:88cbc65f2563 136 return;
PatrickZieverink 9:6537eead1241 137 }
joostbonekamp 12:88cbc65f2563 138 void homing()
joostbonekamp 12:88cbc65f2563 139 /*
joostbonekamp 14:4cf17b10e504 140 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 141 */
joostbonekamp 12:88cbc65f2563 142 {
joostbonekamp 12:88cbc65f2563 143 if (state_changed) {
joostbonekamp 12:88cbc65f2563 144 pc.printf("Started homing");
joostbonekamp 12:88cbc65f2563 145 state_changed = false;
joostbonekamp 12:88cbc65f2563 146 }
joostbonekamp 12:88cbc65f2563 147 return;
joostbonekamp 12:88cbc65f2563 148 }
PatrickZieverink 9:6537eead1241 149
joostbonekamp 12:88cbc65f2563 150 void measure_signals()
joostbonekamp 12:88cbc65f2563 151 {
joostbonekamp 16:696e9cbcc823 152 enc1_value = enc1.getPulses();
joostbonekamp 16:696e9cbcc823 153 enc2_value = enc2.getPulses();
joostbonekamp 16:696e9cbcc823 154 if (enc_has_been_calibrated) {
joostbonekamp 16:696e9cbcc823 155 enc1_value -= enc1_zero;
joostbonekamp 16:696e9cbcc823 156 enc2_value -= enc2_zero;
joostbonekamp 16:696e9cbcc823 157 }
joostbonekamp 16:696e9cbcc823 158 EMG1_raw = EMG1.read();
joostbonekamp 16:696e9cbcc823 159 EMG2_raw = EMG2.read();
joostbonekamp 12:88cbc65f2563 160 }
joostbonekamp 10:b8c60fd468f1 161
joostbonekamp 15:9a1f34bc9958 162 void output()
joostbonekamp 14:4cf17b10e504 163 {
joostbonekamp 16:696e9cbcc823 164 motor1_direction = actuator.dir1;
joostbonekamp 16:696e9cbcc823 165 motor2_direction = acuator.dir2;
joostbonekamp 16:696e9cbcc823 166 motor1_pwm.write(actuator.pwm1);
joostbonekamp 16:696e9cbcc823 167 motor2_pwm.write(actuator.pwm2);
joostbonekamp 15:9a1f34bc9958 168 }
joostbonekamp 14:4cf17b10e504 169
joostbonekamp 15:9a1f34bc9958 170 void state_machine()
joostbonekamp 15:9a1f34bc9958 171 {
joostbonekamp 16:696e9cbcc823 172 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 173 //run current state
joostbonekamp 15:9a1f34bc9958 174 switch (current_state) {
joostbonekamp 15:9a1f34bc9958 175 case idle:
joostbonekamp 15:9a1f34bc9958 176 do_nothing();
joostbonekamp 15:9a1f34bc9958 177 break;
joostbonekamp 15:9a1f34bc9958 178 case failure:
joostbonekamp 15:9a1f34bc9958 179 failure();
joostbonekamp 15:9a1f34bc9958 180 break;
joostbonekamp 15:9a1f34bc9958 181 case cali_EMG:
joostbonekamp 15:9a1f34bc9958 182 cali_EMG();
joostbonekamp 15:9a1f34bc9958 183 break;
joostbonekamp 15:9a1f34bc9958 184 case cali_ENC:
joostbonekamp 15:9a1f34bc9958 185 cali_encoder();
joostbonekamp 15:9a1f34bc9958 186 break;
joostbonekamp 15:9a1f34bc9958 187 case moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 188 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 189 break;
joostbonekamp 15:9a1f34bc9958 190 case moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 191 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 192 break;
joostbonekamp 15:9a1f34bc9958 193 case homing:
joostbonekamp 15:9a1f34bc9958 194 homing();
joostbonekamp 15:9a1f34bc9958 195 break;
joostbonekamp 5:aa8b5d5e632f 196 }
joostbonekamp 5:aa8b5d5e632f 197 }
joostbonekamp 15:9a1f34bc9958 198
joostbonekamp 15:9a1f34bc9958 199 void main_loop()
joostbonekamp 12:88cbc65f2563 200 {
joostbonekamp 15:9a1f34bc9958 201 measure_signals();
joostbonekamp 15:9a1f34bc9958 202 state_machine();
joostbonekamp 15:9a1f34bc9958 203 motor_controller();
joostbonekamp 15:9a1f34bc9958 204 output();
joostbonekamp 15:9a1f34bc9958 205 }
joostbonekamp 14:4cf17b10e504 206
joostbonekamp 14:4cf17b10e504 207 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 208 //state machines
joostbonekamp 16:696e9cbcc823 209 void check_failure()
joostbonekamp 15:9a1f34bc9958 210 {
joostbonekamp 16:696e9cbcc823 211 state = failure;
joostbonekamp 16:696e9cbcc823 212 state_changed = true;
joostbonekamp 16:696e9cbcc823 213 }
joostbonekamp 16:696e9cbcc823 214
joostbonekamp 16:696e9cbcc823 215 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 216 {
joostbonekamp 16:696e9cbcc823 217 if(button2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 218 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 219 }
joostbonekamp 15:9a1f34bc9958 220 but1_pressed = true;
joostbonekamp 15:9a1f34bc9958 221 pc.printf("Button 1 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 222 }
joostbonekamp 14:4cf17b10e504 223
joostbonekamp 16:696e9cbcc823 224 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 225 {
joostbonekamp 16:696e9cbcc823 226 if(button1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 227 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 228 }
joostbonekamp 15:9a1f34bc9958 229 but2_pressed = true;
joostbonekamp 15:9a1f34bc9958 230 pc.printf("Button 2 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 231 }
joostbonekamp 16:696e9cbcc823 232 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 233 {
joostbonekamp 16:696e9cbcc823 234 speed = -1; //default value, this means the state should not change
joostbonekamp 16:696e9cbcc823 235 float levels = 5.0;
joostbonekamp 16:696e9cbcc823 236 if (i > 0/14 && i < 2/14) {speed = 0;}
joostbonekamp 16:696e9cbcc823 237 if (i > 3/14 && i < 5/14) {speed = 1;}
joostbonekamp 16:696e9cbcc823 238 if (i > 6/14 && i < 8/14} (speed = 2;}
joostbonekamp 16:696e9cbcc823 239 if (i > 9/14 && i < 11/14) {speed = 3;}
joostbonekamp 16:696e9cbcc823 240 if (i > 12/14 && i < 14/14) {speed = 4;}
joostbonekamp 16:696e9cbcc823 241 return speed;
joostbonekamp 16:696e9cbcc823 242 }
joostbonekamp 14:4cf17b10e504 243
joostbonekamp 15:9a1f34bc9958 244 int main()
joostbonekamp 15:9a1f34bc9958 245 {
joostbonekamp 15:9a1f34bc9958 246 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 247 pc.printf("Executing main()... \r\n");
joostbonekamp 15:9a1f34bc9958 248 current_state = idle;
joostbonekamp 12:88cbc65f2563 249
joostbonekamp 15:9a1f34bc9958 250 motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait
joostbonekamp 15:9a1f34bc9958 251 motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 252
joostbonekamp 15:9a1f34bc9958 253 actuator.dir1 = 0;
joostbonekamp 15:9a1f34bc9958 254 actuator.dir2 = 0;
joostbonekamp 15:9a1f34bc9958 255
joostbonekamp 15:9a1f34bc9958 256 actuator.magnet = false;
joostbonekamp 14:4cf17b10e504 257
joostbonekamp 15:9a1f34bc9958 258 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 259 but2.fall(&but2_interrupt);
joostbonekamp 15:9a1f34bc9958 260 loop_ticker.attach(&main_loop, 0.001f); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 261 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 262 while (true) {
joostbonekamp 16:696e9cbcc823 263 wait(0.1f);
joostbonekamp 16:696e9cbcc823 264 }
joostbonekamp 16:696e9cbcc823 265 }]