Hidde van der Bijl / Mbed 2 deprecated biorobotics_four_scorers2

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@26:6af46ad1e4ea, 2019-10-24 (annotated)

Committer:
hidde1104
Date:
Thu Oct 24 12:21:01 2019 +0000
Revision:
26:6af46ad1e4ea
Parent:
23:9eeac9d1ecbe 
Boundaries & snelheid waarde toekenning

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joostbonekamp 18:dddc8d9f7638 1 /*
joostbonekamp 18:dddc8d9f7638 2 To-do:
joostbonekamp 18:dddc8d9f7638 3 Add reference generator
joostbonekamp 18:dddc8d9f7638 4 fully implement schmitt trigger
joostbonekamp 18:dddc8d9f7638 5 Homing
joostbonekamp 18:dddc8d9f7638 6 Turning the magnet on/off
joostbonekamp 18:dddc8d9f7638 7 Inverse kinematics
joostbonekamp 18:dddc8d9f7638 8 Gravity compensation
joostbonekamp 18:dddc8d9f7638 9 PID values
joostbonekamp 18:dddc8d9f7638 10 General program layout
joostbonekamp 19:a37cae6964ca 11 better names for EMG input
joostbonekamp 18:dddc8d9f7638 12 */
joostbonekamp 18:dddc8d9f7638 13
RobertoO 0:67c50348f842 14 #include "mbed.h"
RobertoO 1:b862262a9d14 15 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 16 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 17 #include "QEI.h"
joostbonekamp 17:615c5d8b3710 18 #include "HIDScope.h"
joostbonekamp 17:615c5d8b3710 19 #include "BiQuad.h"
hidde1104 13:51ae2da8da55 20 #define PI 3.14159265
RobertoO 0:67c50348f842 21
joostbonekamp 12:88cbc65f2563 22 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 23:9eeac9d1ecbe 23 HIDScope scope(2); //HIDScope instance
joostbonekamp 12:88cbc65f2563 24 DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 12:88cbc65f2563 25 FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 12:88cbc65f2563 26 DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 12:88cbc65f2563 27 FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 28 Ticker loop_ticker; //used in main()
joostbonekamp 20:4424d447f3cd 29 Ticker scope_ticker;
joostbonekamp 23:9eeac9d1ecbe 30 InterruptIn but1(SW2); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 31 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 23:9eeac9d1ecbe 32 AnalogIn EMG1(A0);
joostbonekamp 23:9eeac9d1ecbe 33 AnalogIn EMG2(A1);
joostbonekamp 12:88cbc65f2563 34
joostbonekamp 17:615c5d8b3710 35 void check_failure();
joostbonekamp 23:9eeac9d1ecbe 36 int schmitt_trigger(float i);
joostbonekamp 10:b8c60fd468f1 37
joostbonekamp 17:615c5d8b3710 38 QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 17:615c5d8b3710 39 QEI enc2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 17:615c5d8b3710 40
joostbonekamp 17:615c5d8b3710 41 BiQuad bq1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 23:9eeac9d1ecbe 42 BiQuad bq2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
joostbonekamp 3:e3d12393adb1 43
joostbonekamp 5:aa8b5d5e632f 44 //variables
joostbonekamp 17:615c5d8b3710 45 enum States {s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
joostbonekamp 12:88cbc65f2563 46 States state; //using the States enum
joostbonekamp 14:4cf17b10e504 47 struct actuator_state {
joostbonekamp 12:88cbc65f2563 48 float duty_cycle1; //pwm of 1st motor
joostbonekamp 12:88cbc65f2563 49 float duty_cycle2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 50 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 51 int dir2; //direction of 2nd motor
joostbonekamp 12:88cbc65f2563 52 bool magnet; //state of the magnet
joostbonekamp 17:615c5d8b3710 53 } actuator;
joostbonekamp 12:88cbc65f2563 54
joostbonekamp 14:4cf17b10e504 55 struct EMG_params {
joostbonekamp 16:696e9cbcc823 56 float max; //params of the emg, tbd during calibration
joostbonekamp 17:615c5d8b3710 57 float min;
joostbonekamp 16:696e9cbcc823 58 } EMG_values;
joostbonekamp 5:aa8b5d5e632f 59
joostbonekamp 18:dddc8d9f7638 60 struct PID {
joostbonekamp 18:dddc8d9f7638 61 float P;
joostbonekamp 18:dddc8d9f7638 62 float I;
joostbonekamp 18:dddc8d9f7638 63 float D;
joostbonekamp 18:dddc8d9f7638 64 float I_counter;
joostbonekamp 19:a37cae6964ca 65 };
joostbonekamp 19:a37cae6964ca 66 PID PID1;
joostbonekamp 19:a37cae6964ca 67 PID PID2;
joostbonekamp 18:dddc8d9f7638 68
joostbonekamp 18:dddc8d9f7638 69 float dt = 0.001;
joostbonekamp 19:a37cae6964ca 70 float theta;
joostbonekamp 19:a37cae6964ca 71 float L;
joostbonekamp 20:4424d447f3cd 72 int enc1_zero = 0;//the zero position of the encoders, to be determined from the
joostbonekamp 20:4424d447f3cd 73 int enc2_zero = 0;//encoder calibration
joostbonekamp 23:9eeac9d1ecbe 74 float EMG1_filtered;
joostbonekamp 23:9eeac9d1ecbe 75 float EMG2_filtered;
joostbonekamp 16:696e9cbcc823 76 int enc1_value;
joostbonekamp 16:696e9cbcc823 77 int enc2_value;
joostbonekamp 18:dddc8d9f7638 78 float error1 = 0.0;
joostbonekamp 18:dddc8d9f7638 79 float error2 = 0.0;
joostbonekamp 19:a37cae6964ca 80 float last_error1 = 0.0;
joostbonekamp 19:a37cae6964ca 81 float last_error2 = 0.0;
joostbonekamp 19:a37cae6964ca 82 float action1;
joostbonekamp 19:a37cae6964ca 83 float action2;
joostbonekamp 12:88cbc65f2563 84 bool state_changed = false; //used to see if the state is "starting"
PatrickZieverink 8:6f6a4dc12036 85 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 86 volatile bool but2_pressed = false;
joostbonekamp 16:696e9cbcc823 87 volatile bool failure_occurred = false;
joostbonekamp 16:696e9cbcc823 88 bool EMG_has_been_calibrated;
joostbonekamp 17:615c5d8b3710 89 bool button1_pressed;
joostbonekamp 17:615c5d8b3710 90 bool button2_pressed;
joostbonekamp 23:9eeac9d1ecbe 91 float filter_value1;
joostbonekamp 23:9eeac9d1ecbe 92 int past_speed = 0;
joostbonekamp 12:88cbc65f2563 93
joostbonekamp 12:88cbc65f2563 94 void do_nothing()
joostbonekamp 5:aa8b5d5e632f 95
PatrickZieverink 9:6537eead1241 96 /*
joostbonekamp 12:88cbc65f2563 97 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 98 */
joostbonekamp 16:696e9cbcc823 99 {
joostbonekamp 16:696e9cbcc823 100 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 101 state_changed = true;
joostbonekamp 23:9eeac9d1ecbe 102 state = s_cali_EMG;
joostbonekamp 16:696e9cbcc823 103 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 104 }
joostbonekamp 16:696e9cbcc823 105 }
joostbonekamp 12:88cbc65f2563 106
joostbonekamp 12:88cbc65f2563 107 void failure()
joostbonekamp 12:88cbc65f2563 108 /*
joostbonekamp 12:88cbc65f2563 109 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 110 */
joostbonekamp 12:88cbc65f2563 111 {
joostbonekamp 12:88cbc65f2563 112 if (state_changed) {
joostbonekamp 12:88cbc65f2563 113 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 114 state_changed = false;
joostbonekamp 12:88cbc65f2563 115 }
joostbonekamp 12:88cbc65f2563 116 }
PatrickZieverink 9:6537eead1241 117
joostbonekamp 23:9eeac9d1ecbe 118 const int EMG_cali_amount = 1000;
joostbonekamp 23:9eeac9d1ecbe 119 float past_EMG_values[EMG_cali_amount];
joostbonekamp 23:9eeac9d1ecbe 120 int past_EMG_count = 0;
joostbonekamp 23:9eeac9d1ecbe 121
joostbonekamp 12:88cbc65f2563 122 void cali_EMG()
joostbonekamp 12:88cbc65f2563 123 /*
joostbonekamp 16:696e9cbcc823 124 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 125 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 126 */
joostbonekamp 12:88cbc65f2563 127 {
joostbonekamp 12:88cbc65f2563 128 if (state_changed) {
joostbonekamp 23:9eeac9d1ecbe 129 pc.printf("Started EMG calibration\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 12:88cbc65f2563 130 state_changed = false;
PatrickZieverink 9:6537eead1241 131 }
joostbonekamp 19:a37cae6964ca 132 if (past_EMG_count < EMG_cali_amount) {
joostbonekamp 19:a37cae6964ca 133 past_EMG_values[past_EMG_count] = EMG1_filtered;
joostbonekamp 19:a37cae6964ca 134 past_EMG_count++;
joostbonekamp 19:a37cae6964ca 135 }
joostbonekamp 23:9eeac9d1ecbe 136 else { //calibration has concluded
joostbonekamp 23:9eeac9d1ecbe 137 pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 23:9eeac9d1ecbe 138 float highest = 0.0;
joostbonekamp 19:a37cae6964ca 139 float sum = 0.0;
joostbonekamp 19:a37cae6964ca 140 for(int i = 0; i<EMG_cali_amount; i++) {
joostbonekamp 19:a37cae6964ca 141 sum += past_EMG_values[i];
joostbonekamp 19:a37cae6964ca 142 }
joostbonekamp 19:a37cae6964ca 143 float mean = sum/(float)EMG_cali_amount;
joostbonekamp 19:a37cae6964ca 144 EMG_values.min = mean;
joostbonekamp 19:a37cae6964ca 145 //calibration done, moving to cali_enc
joostbonekamp 23:9eeac9d1ecbe 146 pc.printf("Min value: %f\r\n", EMG_values.min);
joostbonekamp 23:9eeac9d1ecbe 147 pc.printf("Length: %f\r\n", (float)EMG_cali_amount);
joostbonekamp 23:9eeac9d1ecbe 148 pc.printf("Calibration of the EMG is done, lower bound = %f\r\n", mean);
joostbonekamp 19:a37cae6964ca 149 EMG_has_been_calibrated = true;
joostbonekamp 19:a37cae6964ca 150 state_changed = true;
joostbonekamp 19:a37cae6964ca 151 state = s_cali_enc;
joostbonekamp 19:a37cae6964ca 152 }
joostbonekamp 12:88cbc65f2563 153 }
joostbonekamp 16:696e9cbcc823 154
joostbonekamp 12:88cbc65f2563 155 void cali_enc()
joostbonekamp 12:88cbc65f2563 156 /*
joostbonekamp 14:4cf17b10e504 157 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 19:a37cae6964ca 158 position for the linear stage and the horizontal postition for the
joostbonekamp 12:88cbc65f2563 159 rotating stage.
joostbonekamp 12:88cbc65f2563 160 */
joostbonekamp 12:88cbc65f2563 161 {
joostbonekamp 12:88cbc65f2563 162 if (state_changed) {
joostbonekamp 12:88cbc65f2563 163 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 164 state_changed = false;
PatrickZieverink 9:6537eead1241 165 }
joostbonekamp 16:696e9cbcc823 166 if (button1_pressed) {
joostbonekamp 19:a37cae6964ca 167 pc.printf("Encoder has been calibrated");
joostbonekamp 16:696e9cbcc823 168 enc1_zero = enc1_value;
joostbonekamp 16:696e9cbcc823 169 enc2_zero = enc2_value;
joostbonekamp 16:696e9cbcc823 170 button1_pressed = false;
joostbonekamp 17:615c5d8b3710 171 state = s_moving_magnet_off;
joostbonekamp 16:696e9cbcc823 172 state_changed = true;
joostbonekamp 16:696e9cbcc823 173
joostbonekamp 16:696e9cbcc823 174 }
joostbonekamp 12:88cbc65f2563 175 }
joostbonekamp 16:696e9cbcc823 176
hidde1104 26:6af46ad1e4ea 177 float x;
hidde1104 26:6af46ad1e4ea 178 float y;
hidde1104 26:6af46ad1e4ea 179 float vx;
hidde1104 26:6af46ad1e4ea 180 float vy;
hidde1104 26:6af46ad1e4ea 181 float q1;
hidde1104 26:6af46ad1e4ea 182 float q2;
hidde1104 26:6af46ad1e4ea 183 float q_dot_2;
hidde1104 26:6af46ad1e4ea 184
hidde1104 26:6af46ad1e4ea 185 void playing_field()
hidde1104 26:6af46ad1e4ea 186 {
hidde1104 26:6af46ad1e4ea 187 x = q2*cos(q1);
hidde1104 26:6af46ad1e4ea 188 y = q2*sin(q1);
hidde1104 26:6af46ad1e4ea 189 if (x <= 0.6 && y <= 0.28){
hidde1104 26:6af46ad1e4ea 190 if (vx < 0.0){
hidde1104 26:6af46ad1e4ea 191 vx = 0.0;
hidde1104 26:6af46ad1e4ea 192 }
hidde1104 26:6af46ad1e4ea 193 if (vy < 0.0){
hidde1104 26:6af46ad1e4ea 194 vy = 0.0;
hidde1104 26:6af46ad1e4ea 195 }
hidde1104 26:6af46ad1e4ea 196 }
hidde1104 26:6af46ad1e4ea 197 if (y > 0.45 || q2 > 0.63){
hidde1104 26:6af46ad1e4ea 198 if (vy > 0.0){
hidde1104 26:6af46ad1e4ea 199 vy = 0.0;
hidde1104 26:6af46ad1e4ea 200 }
hidde1104 26:6af46ad1e4ea 201 if (q2 > 0.63) {
hidde1104 26:6af46ad1e4ea 202 q_dot_2 = 0.0;
hidde1104 26:6af46ad1e4ea 203 }
hidde1104 26:6af46ad1e4ea 204 }
hidde1104 26:6af46ad1e4ea 205 }
hidde1104 26:6af46ad1e4ea 206
hidde1104 26:6af46ad1e4ea 207
hidde1104 26:6af46ad1e4ea 208
hidde1104 26:6af46ad1e4ea 209
hidde1104 26:6af46ad1e4ea 210
joostbonekamp 12:88cbc65f2563 211 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 212 /*
joostbonekamp 14:4cf17b10e504 213 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 214 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 215 */
joostbonekamp 12:88cbc65f2563 216 {
joostbonekamp 12:88cbc65f2563 217 if (state_changed) {
joostbonekamp 12:88cbc65f2563 218 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 219 state_changed = false;
PatrickZieverink 9:6537eead1241 220 }
PatrickZieverink 9:6537eead1241 221 }
joostbonekamp 16:696e9cbcc823 222
joostbonekamp 12:88cbc65f2563 223 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 224 /*
joostbonekamp 14:4cf17b10e504 225 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 226 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 227 */
joostbonekamp 12:88cbc65f2563 228 {
joostbonekamp 12:88cbc65f2563 229 if (state_changed) {
joostbonekamp 12:88cbc65f2563 230 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 231 state_changed = false;
joostbonekamp 12:88cbc65f2563 232 }
joostbonekamp 12:88cbc65f2563 233 return;
PatrickZieverink 9:6537eead1241 234 }
joostbonekamp 12:88cbc65f2563 235 void homing()
joostbonekamp 12:88cbc65f2563 236 /*
joostbonekamp 14:4cf17b10e504 237 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 238 */
joostbonekamp 12:88cbc65f2563 239 {
joostbonekamp 12:88cbc65f2563 240 if (state_changed) {
joostbonekamp 12:88cbc65f2563 241 pc.printf("Started homing");
joostbonekamp 12:88cbc65f2563 242 state_changed = false;
joostbonekamp 12:88cbc65f2563 243 }
joostbonekamp 12:88cbc65f2563 244 return;
joostbonekamp 12:88cbc65f2563 245 }
PatrickZieverink 9:6537eead1241 246
joostbonekamp 23:9eeac9d1ecbe 247 float EMG1_raw;
joostbonekamp 23:9eeac9d1ecbe 248 float EMG2_raw;
joostbonekamp 12:88cbc65f2563 249 void measure_signals()
joostbonekamp 12:88cbc65f2563 250 {
joostbonekamp 19:a37cae6964ca 251 //only one emg input, reference and plus
joostbonekamp 23:9eeac9d1ecbe 252 EMG1_raw = EMG1.read();
joostbonekamp 23:9eeac9d1ecbe 253 EMG2_raw = EMG2.read();
joostbonekamp 23:9eeac9d1ecbe 254 filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
joostbonekamp 19:a37cae6964ca 255
joostbonekamp 19:a37cae6964ca 256 if (filter_value1 > EMG_values.max) {
joostbonekamp 19:a37cae6964ca 257 EMG_values.max = filter_value1;
joostbonekamp 19:a37cae6964ca 258 }
joostbonekamp 19:a37cae6964ca 259 if (EMG_has_been_calibrated) {
joostbonekamp 19:a37cae6964ca 260 EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min);
joostbonekamp 19:a37cae6964ca 261 }
joostbonekamp 19:a37cae6964ca 262 else {
joostbonekamp 19:a37cae6964ca 263 EMG1_filtered = filter_value1;
joostbonekamp 19:a37cae6964ca 264 }
joostbonekamp 19:a37cae6964ca 265
joostbonekamp 16:696e9cbcc823 266 enc1_value = enc1.getPulses();
joostbonekamp 16:696e9cbcc823 267 enc2_value = enc2.getPulses();
joostbonekamp 19:a37cae6964ca 268 enc1_value -= enc1_zero;
joostbonekamp 19:a37cae6964ca 269 enc2_value -= enc2_zero;
joostbonekamp 19:a37cae6964ca 270 theta = (float)(enc1_value)/(float)(8400*2*PI);
joostbonekamp 19:a37cae6964ca 271 L = (float)(enc2_value)/(float)(8400*2*PI);
joostbonekamp 19:a37cae6964ca 272
joostbonekamp 19:a37cae6964ca 273
joostbonekamp 12:88cbc65f2563 274 }
joostbonekamp 10:b8c60fd468f1 275
joostbonekamp 23:9eeac9d1ecbe 276 void motor_controller() {
joostbonekamp 23:9eeac9d1ecbe 277 int speed = schmitt_trigger(EMG1_filtered);
joostbonekamp 23:9eeac9d1ecbe 278 if (speed == -1) {speed = past_speed;}
joostbonekamp 23:9eeac9d1ecbe 279 past_speed = speed;
joostbonekamp 17:615c5d8b3710 280
joostbonekamp 17:615c5d8b3710 281
joostbonekamp 18:dddc8d9f7638 282 float error1, error2;
joostbonekamp 18:dddc8d9f7638 283 //P part of the controller
joostbonekamp 18:dddc8d9f7638 284 float P_action1 = PID1.P * error1;
joostbonekamp 18:dddc8d9f7638 285 float P_action2 = PID2.P * error2;
joostbonekamp 17:615c5d8b3710 286
joostbonekamp 18:dddc8d9f7638 287 //I part
joostbonekamp 18:dddc8d9f7638 288 PID1.I_counter += error1;
joostbonekamp 18:dddc8d9f7638 289 PID2.I_counter += error2;
joostbonekamp 18:dddc8d9f7638 290 float I_action1 = PID1.I_counter * PID1.I;
joostbonekamp 18:dddc8d9f7638 291 float I_action2 = PID2.I_counter * PID2.I;
joostbonekamp 18:dddc8d9f7638 292
joostbonekamp 18:dddc8d9f7638 293 //D part
joostbonekamp 18:dddc8d9f7638 294 float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
joostbonekamp 18:dddc8d9f7638 295 float velocity_estimate_2 = (error2-last_error2)/dt;
joostbonekamp 18:dddc8d9f7638 296 float D_action1 = velocity_estimate_1 * PID1.D;
joostbonekamp 18:dddc8d9f7638 297 float D_action2 = velocity_estimate_2 * PID2.D;
joostbonekamp 18:dddc8d9f7638 298
joostbonekamp 18:dddc8d9f7638 299 action1 = P_action1 + I_action1 + D_action1;
joostbonekamp 18:dddc8d9f7638 300 action2 = P_action2 + I_action2 + D_action2;
joostbonekamp 18:dddc8d9f7638 301
joostbonekamp 18:dddc8d9f7638 302 last_error1 = error1;
joostbonekamp 18:dddc8d9f7638 303 last_error2 = error2;
joostbonekamp 17:615c5d8b3710 304 }
joostbonekamp 17:615c5d8b3710 305
joostbonekamp 15:9a1f34bc9958 306 void output()
joostbonekamp 14:4cf17b10e504 307 {
joostbonekamp 16:696e9cbcc823 308 motor1_direction = actuator.dir1;
joostbonekamp 17:615c5d8b3710 309 motor2_direction = actuator.dir2;
joostbonekamp 17:615c5d8b3710 310 motor1_pwm.write(actuator.duty_cycle1);
joostbonekamp 17:615c5d8b3710 311 motor2_pwm.write(actuator.duty_cycle2);
joostbonekamp 20:4424d447f3cd 312
joostbonekamp 20:4424d447f3cd 313 scope.set(0, EMG1_filtered);
joostbonekamp 23:9eeac9d1ecbe 314 scope.set(1, past_speed);
joostbonekamp 15:9a1f34bc9958 315 }
joostbonekamp 14:4cf17b10e504 316
joostbonekamp 15:9a1f34bc9958 317 void state_machine()
joostbonekamp 15:9a1f34bc9958 318 {
joostbonekamp 16:696e9cbcc823 319 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 320 //run current state
joostbonekamp 17:615c5d8b3710 321 switch (state) {
joostbonekamp 17:615c5d8b3710 322 case s_idle:
joostbonekamp 15:9a1f34bc9958 323 do_nothing();
joostbonekamp 15:9a1f34bc9958 324 break;
joostbonekamp 17:615c5d8b3710 325 case s_failure:
joostbonekamp 15:9a1f34bc9958 326 failure();
joostbonekamp 15:9a1f34bc9958 327 break;
joostbonekamp 17:615c5d8b3710 328 case s_cali_EMG:
joostbonekamp 15:9a1f34bc9958 329 cali_EMG();
joostbonekamp 15:9a1f34bc9958 330 break;
joostbonekamp 17:615c5d8b3710 331 case s_cali_enc:
joostbonekamp 17:615c5d8b3710 332 cali_enc();
joostbonekamp 15:9a1f34bc9958 333 break;
joostbonekamp 17:615c5d8b3710 334 case s_moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 335 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 336 break;
joostbonekamp 17:615c5d8b3710 337 case s_moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 338 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 339 break;
joostbonekamp 17:615c5d8b3710 340 case s_homing:
joostbonekamp 15:9a1f34bc9958 341 homing();
joostbonekamp 15:9a1f34bc9958 342 break;
joostbonekamp 5:aa8b5d5e632f 343 }
joostbonekamp 5:aa8b5d5e632f 344 }
joostbonekamp 15:9a1f34bc9958 345
joostbonekamp 15:9a1f34bc9958 346 void main_loop()
joostbonekamp 12:88cbc65f2563 347 {
joostbonekamp 15:9a1f34bc9958 348 measure_signals();
joostbonekamp 15:9a1f34bc9958 349 state_machine();
joostbonekamp 15:9a1f34bc9958 350 motor_controller();
joostbonekamp 15:9a1f34bc9958 351 output();
joostbonekamp 15:9a1f34bc9958 352 }
joostbonekamp 14:4cf17b10e504 353
joostbonekamp 14:4cf17b10e504 354 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 355 //state machines
joostbonekamp 16:696e9cbcc823 356 void check_failure()
joostbonekamp 15:9a1f34bc9958 357 {
joostbonekamp 21:bea7c8a08e1d 358 //state = s_failure;
joostbonekamp 21:bea7c8a08e1d 359 //state_changed = true;
joostbonekamp 16:696e9cbcc823 360 }
joostbonekamp 16:696e9cbcc823 361
joostbonekamp 16:696e9cbcc823 362 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 363 {
joostbonekamp 17:615c5d8b3710 364 if(but2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 365 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 366 }
joostbonekamp 23:9eeac9d1ecbe 367 button1_pressed = true;
joostbonekamp 15:9a1f34bc9958 368 pc.printf("Button 1 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 369 }
joostbonekamp 14:4cf17b10e504 370
joostbonekamp 16:696e9cbcc823 371 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 372 {
joostbonekamp 17:615c5d8b3710 373 if(but1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 374 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 375 }
joostbonekamp 23:9eeac9d1ecbe 376 button2_pressed = true;
joostbonekamp 15:9a1f34bc9958 377 pc.printf("Button 2 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 378 }
joostbonekamp 17:615c5d8b3710 379
joostbonekamp 16:696e9cbcc823 380 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 381 {
joostbonekamp 17:615c5d8b3710 382 int speed;
joostbonekamp 16:696e9cbcc823 383 speed = -1; //default value, this means the state should not change
joostbonekamp 23:9eeac9d1ecbe 384 if (i > 0.000 && i < 0.125) {speed = 0;}
joostbonekamp 23:9eeac9d1ecbe 385 if (i > 0.250 && i < 0.375) {speed = 1;}
joostbonekamp 23:9eeac9d1ecbe 386 if (i > 0.500 && i < 1.000) {speed = 2;}
joostbonekamp 16:696e9cbcc823 387 return speed;
joostbonekamp 16:696e9cbcc823 388 }
joostbonekamp 14:4cf17b10e504 389
hidde1104 26:6af46ad1e4ea 390 float v[2] = {vx, vy};
hidde1104 26:6af46ad1e4ea 391
hidde1104 26:6af46ad1e4ea 392 void velocity_control() {
hidde1104 26:6af46ad1e4ea 393 int c;
hidde1104 26:6af46ad1e4ea 394 for (c = 0; c<2; c++) {
hidde1104 26:6af46ad1e4ea 395 if (speed[c] == 0){
hidde1104 26:6af46ad1e4ea 396 v[c] = 0;
hidde1104 26:6af46ad1e4ea 397 }
hidde1104 26:6af46ad1e4ea 398 if (speed[c] == 1){
hidde1104 26:6af46ad1e4ea 399 v[c] = 0.01;
hidde1104 26:6af46ad1e4ea 400 }
hidde1104 26:6af46ad1e4ea 401 if (speed[c] == 2){
hidde1104 26:6af46ad1e4ea 402 v[c] = 0.02;
hidde1104 26:6af46ad1e4ea 403 }
hidde1104 26:6af46ad1e4ea 404 }
hidde1104 26:6af46ad1e4ea 405
joostbonekamp 15:9a1f34bc9958 406 int main()
joostbonekamp 15:9a1f34bc9958 407 {
joostbonekamp 15:9a1f34bc9958 408 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 409 pc.printf("Executing main()... \r\n");
joostbonekamp 17:615c5d8b3710 410 state = s_idle;
joostbonekamp 12:88cbc65f2563 411
joostbonekamp 17:615c5d8b3710 412 motor2_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 17:615c5d8b3710 413 motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 414
joostbonekamp 15:9a1f34bc9958 415 actuator.dir1 = 0;
joostbonekamp 15:9a1f34bc9958 416 actuator.dir2 = 0;
joostbonekamp 15:9a1f34bc9958 417
joostbonekamp 15:9a1f34bc9958 418 actuator.magnet = false;
joostbonekamp 23:9eeac9d1ecbe 419 EMG_values.max = 0.01;
joostbonekamp 14:4cf17b10e504 420
joostbonekamp 15:9a1f34bc9958 421 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 422 but2.fall(&but2_interrupt);
joostbonekamp 20:4424d447f3cd 423 scope_ticker.attach(&scope, &HIDScope::send, 0.02);
joostbonekamp 18:dddc8d9f7638 424 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 425 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 426 while (true) {
joostbonekamp 16:696e9cbcc823 427 wait(0.1f);
joostbonekamp 16:696e9cbcc823 428 }
joostbonekamp 17:615c5d8b3710 429 }