Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_main_not_working

Main werkt niet

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@18:a584af8b4cfb, 2019-10-11 (annotated)

Committer:
PatrickZieverink
Date:
Fri Oct 11 11:05:25 2019 +0000
Revision:
18:a584af8b4cfb
Parent:
17:615c5d8b3710 
Not working Main

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