Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@27:4fa916e1f6a9, 2019-10-28 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 28 09:59:23 2019 +0000
Revision:
27:4fa916e1f6a9
Parent:
26:3456b03d5bce
Child:
28:f08665a5ef6c
upload currently not working

Who changed what in which revision?

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