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@24:7cad312a1e38, 2019-10-23 (annotated)

Committer:
hidde1104
Date:
Wed Oct 23 10:48:46 2019 +0000
Revision:
24:7cad312a1e38
Parent:
23:9eeac9d1ecbe 
niet werkende 2 EMG versie;

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