Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_WERKEND

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:3456b03d5bce, 2019-10-25 (annotated)

Committer:
joostbonekamp
Date:
Fri Oct 25 12:04:46 2019 +0000
Revision:
26:3456b03d5bce
Parent:
25:e1577c9e8c7e
Child:
27:4fa916e1f6a9
Better communication between different parts of the main loop, program should work after adding PID values.

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