Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_21_10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@15:9a1f34bc9958, 2019-10-07 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 07 13:38:54 2019 +0000
Revision:
15:9a1f34bc9958
Parent:
14:4cf17b10e504
Child:
16:696e9cbcc823
removed kinematics from main file, moved them to a separate file;

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