Patrick Zieverink / Mbed 2 deprecated BioRobot_30-10-19

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@0:59335354afee, 2019-10-30 (annotated)

Committer:
PatrickZieverink
Date:
Wed Oct 30 15:27:06 2019 +0000
Revision:
0:59335354afee
Werkend model zonder boundaries 16:27;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PatrickZieverink 0:59335354afee 1 /*
PatrickZieverink 0:59335354afee 2 To-do:
PatrickZieverink 0:59335354afee 3 calibration with reverse kinematics
PatrickZieverink 0:59335354afee 4 EMG calibration upper bound
PatrickZieverink 0:59335354afee 5
PatrickZieverink 0:59335354afee 6 Homing
PatrickZieverink 0:59335354afee 7 Turning the magnet on/off (reading magnet status?)
PatrickZieverink 0:59335354afee 8 Gravity compensation (if necessary)
PatrickZieverink 0:59335354afee 9 PID values (too large)
PatrickZieverink 0:59335354afee 10 Boundaries (after verification of the PID values)
PatrickZieverink 0:59335354afee 11 */
PatrickZieverink 0:59335354afee 12 #include "mbed.h"
PatrickZieverink 0:59335354afee 13 #include "MODSERIAL.h"
PatrickZieverink 0:59335354afee 14 #include "FastPWM.h"
PatrickZieverink 0:59335354afee 15 #include "QEI.h"
PatrickZieverink 0:59335354afee 16 #include "HIDScope.h"
PatrickZieverink 0:59335354afee 17 #include "BiQuad.h"
PatrickZieverink 0:59335354afee 18 #define PI 3.14159265
PatrickZieverink 0:59335354afee 19
PatrickZieverink 0:59335354afee 20 Serial pc(USBTX, USBRX); //connect to pc
PatrickZieverink 0:59335354afee 21 HIDScope scope(5); //HIDScope instance
PatrickZieverink 0:59335354afee 22 DigitalOut motor0_direction(D7); //rotation motor 1 on shield (always D6)
PatrickZieverink 0:59335354afee 23 FastPWM motor0_pwm(D6); //pwm 1 on shield (always D7)
PatrickZieverink 0:59335354afee 24 DigitalOut motor1_direction(D4); //rotation motor 2 on shield (always D4)
PatrickZieverink 0:59335354afee 25 FastPWM motor1_pwm(D5); //pwm 2 on shield (always D5)
PatrickZieverink 0:59335354afee 26 Ticker loop_ticker; //used in main()
PatrickZieverink 0:59335354afee 27 Ticker scope_ticker;
PatrickZieverink 0:59335354afee 28 InterruptIn but1(SW2); //button on mbed board
PatrickZieverink 0:59335354afee 29 InterruptIn but2(D9); //debounced button on biorobotics shield
PatrickZieverink 0:59335354afee 30 InterruptIn but3(D8); //button 1 on bio shield
PatrickZieverink 0:59335354afee 31 AnalogIn EMG0_sig(A0);
PatrickZieverink 0:59335354afee 32 AnalogIn EMG0_ref(A1);
PatrickZieverink 0:59335354afee 33 AnalogIn EMG1_sig(A2);
PatrickZieverink 0:59335354afee 34 AnalogIn EMG1_ref(A3);
PatrickZieverink 0:59335354afee 35
PatrickZieverink 0:59335354afee 36 //Joystick test
PatrickZieverink 0:59335354afee 37
PatrickZieverink 0:59335354afee 38 float direction_switch;
PatrickZieverink 0:59335354afee 39
PatrickZieverink 0:59335354afee 40 void check_failure();
PatrickZieverink 0:59335354afee 41 int schmitt_trigger(float i);
PatrickZieverink 0:59335354afee 42
PatrickZieverink 0:59335354afee 43 QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
PatrickZieverink 0:59335354afee 44 QEI enc2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
PatrickZieverink 0:59335354afee 45
PatrickZieverink 0:59335354afee 46 BiQuad bq1_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
PatrickZieverink 0:59335354afee 47 BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
PatrickZieverink 0:59335354afee 48 BiQuad bq2_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
PatrickZieverink 0:59335354afee 49 BiQuad bq2_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
PatrickZieverink 0:59335354afee 50
PatrickZieverink 0:59335354afee 51 //variables
PatrickZieverink 0:59335354afee 52 enum States {s_idle, s_cali_EMG_low, s_cali_EMG_high, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
PatrickZieverink 0:59335354afee 53 States state; //using the States enum
PatrickZieverink 0:59335354afee 54 struct actuator_state {
PatrickZieverink 0:59335354afee 55 float duty_cycle[2]; //pwm of 1st motor
PatrickZieverink 0:59335354afee 56 bool direction[2]; //direction of 1st motor
PatrickZieverink 0:59335354afee 57 int default_direction[2];
PatrickZieverink 0:59335354afee 58 bool magnet; //state of the magnet
PatrickZieverink 0:59335354afee 59 } actuator;
PatrickZieverink 0:59335354afee 60
PatrickZieverink 0:59335354afee 61 struct EMG_params {
PatrickZieverink 0:59335354afee 62 float max[2]; //params of the emg, tbd during calibration
PatrickZieverink 0:59335354afee 63 float min[2];
PatrickZieverink 0:59335354afee 64 } EMG;
PatrickZieverink 0:59335354afee 65
PatrickZieverink 0:59335354afee 66 struct PID_struct {
PatrickZieverink 0:59335354afee 67 float P[2];
PatrickZieverink 0:59335354afee 68 float I[2];
PatrickZieverink 0:59335354afee 69 float D[2];
PatrickZieverink 0:59335354afee 70 float I_counter[2];
PatrickZieverink 0:59335354afee 71 } PID;
PatrickZieverink 0:59335354afee 72
PatrickZieverink 0:59335354afee 73 float dt = 0.001;
PatrickZieverink 0:59335354afee 74 float theta[2]; //theta0 = rot, theta1 = lin
PatrickZieverink 0:59335354afee 75 int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
PatrickZieverink 0:59335354afee 76 float EMG_raw[2][2];
PatrickZieverink 0:59335354afee 77 float EMG_filtered[2];
PatrickZieverink 0:59335354afee 78 int enc_value[2];
PatrickZieverink 0:59335354afee 79 float current_error[2] = {0.0, 0.0};
PatrickZieverink 0:59335354afee 80 float errors[2];
PatrickZieverink 0:59335354afee 81 float last_error[2] = {0.0, 0.0};
PatrickZieverink 0:59335354afee 82 float action[2];
PatrickZieverink 0:59335354afee 83 bool state_changed = false; //used to see if the state is "starting"
PatrickZieverink 0:59335354afee 84 volatile bool button1_pressed = false;
PatrickZieverink 0:59335354afee 85 volatile bool button2_pressed = false;
PatrickZieverink 0:59335354afee 86 volatile bool button3_pressed = false;
PatrickZieverink 0:59335354afee 87 volatile bool failure_occurred = false;
PatrickZieverink 0:59335354afee 88 bool EMG_low_has_been_calibrated;
PatrickZieverink 0:59335354afee 89 bool EMG_high_has_been_calibrated;
PatrickZieverink 0:59335354afee 90 float filter_value[2];
PatrickZieverink 0:59335354afee 91 int speed[2];
PatrickZieverink 0:59335354afee 92 int past_speed[2] = {0, 0};
PatrickZieverink 0:59335354afee 93 float velocity_desired[2];
PatrickZieverink 0:59335354afee 94 float theta_desired[2];
PatrickZieverink 0:59335354afee 95 const int EMG_cali_amount = 1000;
PatrickZieverink 0:59335354afee 96 float past_EMG_values[2][EMG_cali_amount];
PatrickZieverink 0:59335354afee 97 int past_EMG_count = 0;
PatrickZieverink 0:59335354afee 98
PatrickZieverink 0:59335354afee 99 void do_nothing()
PatrickZieverink 0:59335354afee 100 /*
PatrickZieverink 0:59335354afee 101 Idle state. Used in the beginning, before the calibration states.
PatrickZieverink 0:59335354afee 102 */
PatrickZieverink 0:59335354afee 103 {
PatrickZieverink 0:59335354afee 104 if (button1_pressed) {
PatrickZieverink 0:59335354afee 105 state_changed = true;
PatrickZieverink 0:59335354afee 106 state = s_cali_EMG_low;
PatrickZieverink 0:59335354afee 107 button1_pressed = false;
PatrickZieverink 0:59335354afee 108 }
PatrickZieverink 0:59335354afee 109 errors[0] = 0.0;
PatrickZieverink 0:59335354afee 110 errors[1] = 0.0;
PatrickZieverink 0:59335354afee 111 }
PatrickZieverink 0:59335354afee 112
PatrickZieverink 0:59335354afee 113 void failure()
PatrickZieverink 0:59335354afee 114
PatrickZieverink 0:59335354afee 115 // Failure mode. This should execute when button 2 is pressed during operation.
PatrickZieverink 0:59335354afee 116
PatrickZieverink 0:59335354afee 117 {
PatrickZieverink 0:59335354afee 118 if (state_changed) {
PatrickZieverink 0:59335354afee 119 pc.printf("Something went wrong!\r\n");
PatrickZieverink 0:59335354afee 120 state_changed = false;
PatrickZieverink 0:59335354afee 121 }
PatrickZieverink 0:59335354afee 122 errors[0] = 0.0;
PatrickZieverink 0:59335354afee 123 errors[1] = 0.0;
PatrickZieverink 0:59335354afee 124 PID.I_counter[0] = 0.0;
PatrickZieverink 0:59335354afee 125 PID.I_counter[1] = 0.0;
PatrickZieverink 0:59335354afee 126 }
PatrickZieverink 0:59335354afee 127 void cali_EMG_low()
PatrickZieverink 0:59335354afee 128 /*
PatrickZieverink 0:59335354afee 129 Calibration of the EMG. Values determined during calibration should be
PatrickZieverink 0:59335354afee 130 added to the EMG_params instance.
PatrickZieverink 0:59335354afee 131 */
PatrickZieverink 0:59335354afee 132 {
PatrickZieverink 0:59335354afee 133 if (state_changed) {
PatrickZieverink 0:59335354afee 134 pc.printf("Started low value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
PatrickZieverink 0:59335354afee 135 state_changed = false;
PatrickZieverink 0:59335354afee 136 }
PatrickZieverink 0:59335354afee 137 if (past_EMG_count < EMG_cali_amount) {
PatrickZieverink 0:59335354afee 138 past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
PatrickZieverink 0:59335354afee 139 past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
PatrickZieverink 0:59335354afee 140 past_EMG_count++;
PatrickZieverink 0:59335354afee 141 } else if (!EMG_low_has_been_calibrated) { //calibration has concluded
PatrickZieverink 0:59335354afee 142 pc.printf("Low value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
PatrickZieverink 0:59335354afee 143 float sum[2] = {0.0, 0.0};
PatrickZieverink 0:59335354afee 144 float mean[2] = {0.0, 0.0};
PatrickZieverink 0:59335354afee 145 for(int c = 0; c<2; c++) {
PatrickZieverink 0:59335354afee 146 for(int i = 0; i<EMG_cali_amount; i++) {
PatrickZieverink 0:59335354afee 147 sum[c] += past_EMG_values[c][i];
PatrickZieverink 0:59335354afee 148 }
PatrickZieverink 0:59335354afee 149 mean[c] = sum[c]/(float)EMG_cali_amount;
PatrickZieverink 0:59335354afee 150 EMG.min[c] = mean[c];
PatrickZieverink 0:59335354afee 151 }
PatrickZieverink 0:59335354afee 152
PatrickZieverink 0:59335354afee 153 //calibration done
PatrickZieverink 0:59335354afee 154 pc.printf("Low EMG values: %f %f\r\n", EMG.min[0], EMG.min[1]);
PatrickZieverink 0:59335354afee 155 EMG_low_has_been_calibrated = true;
PatrickZieverink 0:59335354afee 156 }
PatrickZieverink 0:59335354afee 157 errors[0] = 0.0;
PatrickZieverink 0:59335354afee 158 errors[1] = 0.0;
PatrickZieverink 0:59335354afee 159 PID.I_counter[0] = 0.0;
PatrickZieverink 0:59335354afee 160 PID.I_counter[1] = 0.0;
PatrickZieverink 0:59335354afee 161 if (button1_pressed && EMG_low_has_been_calibrated) {// move to high value calibration
PatrickZieverink 0:59335354afee 162 button1_pressed = false;
PatrickZieverink 0:59335354afee 163 state = s_cali_EMG_high;
PatrickZieverink 0:59335354afee 164 state_changed = true;
PatrickZieverink 0:59335354afee 165 }
PatrickZieverink 0:59335354afee 166 }
PatrickZieverink 0:59335354afee 167
PatrickZieverink 0:59335354afee 168 void cali_EMG_high()
PatrickZieverink 0:59335354afee 169 /*
PatrickZieverink 0:59335354afee 170 Calibration of the EMG. Values determined during calibration should be
PatrickZieverink 0:59335354afee 171 added to the EMG_params instance.
PatrickZieverink 0:59335354afee 172 */
PatrickZieverink 0:59335354afee 173 {
PatrickZieverink 0:59335354afee 174 if (state_changed) {
PatrickZieverink 0:59335354afee 175 pc.printf("Started high value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
PatrickZieverink 0:59335354afee 176 state_changed = false;
PatrickZieverink 0:59335354afee 177 past_EMG_count = 0;
PatrickZieverink 0:59335354afee 178 }
PatrickZieverink 0:59335354afee 179 if (past_EMG_count < EMG_cali_amount) {
PatrickZieverink 0:59335354afee 180 past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
PatrickZieverink 0:59335354afee 181 past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
PatrickZieverink 0:59335354afee 182 past_EMG_count++;
PatrickZieverink 0:59335354afee 183 } else { //calibration has concluded
PatrickZieverink 0:59335354afee 184 pc.printf("High value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
PatrickZieverink 0:59335354afee 185 float max[2] = {0.0, 0.0};
PatrickZieverink 0:59335354afee 186 for(int c = 0; c<2; c++) {
PatrickZieverink 0:59335354afee 187 for(int i = 0; i<EMG_cali_amount; i++) {
PatrickZieverink 0:59335354afee 188 if (past_EMG_values[c][i] > max[c]) {
PatrickZieverink 0:59335354afee 189 max[c] = past_EMG_values[c][i];
PatrickZieverink 0:59335354afee 190 }
PatrickZieverink 0:59335354afee 191 }
PatrickZieverink 0:59335354afee 192 EMG.max[c] = max[c];
PatrickZieverink 0:59335354afee 193 }
PatrickZieverink 0:59335354afee 194
PatrickZieverink 0:59335354afee 195 //calibration done, moving to cali_enc
PatrickZieverink 0:59335354afee 196 pc.printf("High EMG values: %f %f\r\n", EMG.max[0], EMG.max[1]);
PatrickZieverink 0:59335354afee 197 EMG_high_has_been_calibrated = true;
PatrickZieverink 0:59335354afee 198 state_changed = true;
PatrickZieverink 0:59335354afee 199 state = s_cali_enc;
PatrickZieverink 0:59335354afee 200 }
PatrickZieverink 0:59335354afee 201 errors[0] = 0.0;
PatrickZieverink 0:59335354afee 202 errors[1] = 0.0;
PatrickZieverink 0:59335354afee 203 PID.I_counter[0] = 0.0;
PatrickZieverink 0:59335354afee 204 PID.I_counter[1] = 0.0;
PatrickZieverink 0:59335354afee 205 }
PatrickZieverink 0:59335354afee 206 void cali_enc()
PatrickZieverink 0:59335354afee 207 /*
PatrickZieverink 0:59335354afee 208 Calibration of the encoder. The encoder should be moved to the lowest
PatrickZieverink 0:59335354afee 209 position for the linear stage and the horizontal postition for the
PatrickZieverink 0:59335354afee 210 rotating stage.
PatrickZieverink 0:59335354afee 211 */
PatrickZieverink 0:59335354afee 212 {
PatrickZieverink 0:59335354afee 213 if (state_changed) {
PatrickZieverink 0:59335354afee 214 pc.printf("Started encoder calibration\r\n");
PatrickZieverink 0:59335354afee 215 state_changed = false;
PatrickZieverink 0:59335354afee 216 }
PatrickZieverink 0:59335354afee 217 if (button1_pressed) {
PatrickZieverink 0:59335354afee 218 pc.printf("Encoder has been calibrated\r\n");
PatrickZieverink 0:59335354afee 219 enc_zero[0] = enc_value[0];
PatrickZieverink 0:59335354afee 220 enc_zero[1] = enc_value[1];//+130990; //the magic number!
PatrickZieverink 0:59335354afee 221 button1_pressed = false;
PatrickZieverink 0:59335354afee 222 state = s_moving_magnet_off;
PatrickZieverink 0:59335354afee 223 state_changed = true;
PatrickZieverink 0:59335354afee 224 }
PatrickZieverink 0:59335354afee 225 errors[0] = 1.0e-5*speed[0];
PatrickZieverink 0:59335354afee 226 errors[1] = 1.0e-5*speed[1];
PatrickZieverink 0:59335354afee 227 }
PatrickZieverink 0:59335354afee 228
PatrickZieverink 0:59335354afee 229 void moving_magnet_off()
PatrickZieverink 0:59335354afee 230 /*
PatrickZieverink 0:59335354afee 231 Moving with the magnet disabled. This is the part from the home position
PatrickZieverink 0:59335354afee 232 towards the storage of chips.
PatrickZieverink 0:59335354afee 233 */
PatrickZieverink 0:59335354afee 234 {
PatrickZieverink 0:59335354afee 235 if (state_changed) {
PatrickZieverink 0:59335354afee 236 pc.printf("Moving without magnet\r\n");
PatrickZieverink 0:59335354afee 237 state_changed = false;
PatrickZieverink 0:59335354afee 238 }
PatrickZieverink 0:59335354afee 239 theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
PatrickZieverink 0:59335354afee 240 theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
PatrickZieverink 0:59335354afee 241 errors[0] = theta_desired[0] - theta[0];
PatrickZieverink 0:59335354afee 242 errors[1] = theta_desired[1] - theta[1];
PatrickZieverink 0:59335354afee 243 if (button2_pressed) {
PatrickZieverink 0:59335354afee 244 pc.printf("positions: (rad, m): %f %f\r\n"
PatrickZieverink 0:59335354afee 245 "Errors: %f %f\r\n"
PatrickZieverink 0:59335354afee 246 "Previous actions: %f %f\r\n"
PatrickZieverink 0:59335354afee 247 "Vx, Vy: %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1], velocity_desired[0],velocity_desired[1]);
PatrickZieverink 0:59335354afee 248 button2_pressed = false;
PatrickZieverink 0:59335354afee 249 }
PatrickZieverink 0:59335354afee 250 }
PatrickZieverink 0:59335354afee 251
PatrickZieverink 0:59335354afee 252 void moving_magnet_on()
PatrickZieverink 0:59335354afee 253
PatrickZieverink 0:59335354afee 254 // Moving with the magnet enabled. This is the part of the movement from the
PatrickZieverink 0:59335354afee 255 // chip holder to the top of the playing board.
PatrickZieverink 0:59335354afee 256
PatrickZieverink 0:59335354afee 257 {
PatrickZieverink 0:59335354afee 258 if (state_changed) {
PatrickZieverink 0:59335354afee 259 pc.printf("Moving with magnet\r\n");
PatrickZieverink 0:59335354afee 260 state_changed = false;
PatrickZieverink 0:59335354afee 261 }
PatrickZieverink 0:59335354afee 262 theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
PatrickZieverink 0:59335354afee 263 theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
PatrickZieverink 0:59335354afee 264 errors[0] = theta_desired[0] - theta[0];
PatrickZieverink 0:59335354afee 265 errors[1] = theta_desired[1] - theta[0];
PatrickZieverink 0:59335354afee 266 }
PatrickZieverink 0:59335354afee 267 void homing()
PatrickZieverink 0:59335354afee 268
PatrickZieverink 0:59335354afee 269 // Dropping the chip and moving towards the rest position.
PatrickZieverink 0:59335354afee 270
PatrickZieverink 0:59335354afee 271 {
PatrickZieverink 0:59335354afee 272 if (state_changed) {
PatrickZieverink 0:59335354afee 273 pc.printf("Started homing\r\n");
PatrickZieverink 0:59335354afee 274 state_changed = false;
PatrickZieverink 0:59335354afee 275 }
PatrickZieverink 0:59335354afee 276 theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
PatrickZieverink 0:59335354afee 277 theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
PatrickZieverink 0:59335354afee 278 errors[0] = theta_desired[0] - theta[0];
PatrickZieverink 0:59335354afee 279 errors[1] = theta_desired[1] - theta[0];
PatrickZieverink 0:59335354afee 280 }
PatrickZieverink 0:59335354afee 281
PatrickZieverink 0:59335354afee 282 void measure_signals()
PatrickZieverink 0:59335354afee 283 {
PatrickZieverink 0:59335354afee 284 //only one emg input, reference and plus
PatrickZieverink 0:59335354afee 285 EMG_raw[0][0] = EMG0_sig.read();
PatrickZieverink 0:59335354afee 286 EMG_raw[0][1] = EMG0_ref.read();
PatrickZieverink 0:59335354afee 287 EMG_raw[1][0] = EMG1_sig.read();
PatrickZieverink 0:59335354afee 288 EMG_raw[1][1] = EMG1_ref.read();
PatrickZieverink 0:59335354afee 289 filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
PatrickZieverink 0:59335354afee 290 filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
PatrickZieverink 0:59335354afee 291
PatrickZieverink 0:59335354afee 292 enc_value[0] = enc1.getPulses();
PatrickZieverink 0:59335354afee 293 enc_value[1] = enc2.getPulses();
PatrickZieverink 0:59335354afee 294
PatrickZieverink 0:59335354afee 295 for(int c = 0; c < 2; c++) {
PatrickZieverink 0:59335354afee 296 if (EMG_high_has_been_calibrated && EMG_low_has_been_calibrated) {
PatrickZieverink 0:59335354afee 297 EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
PatrickZieverink 0:59335354afee 298 } else {
PatrickZieverink 0:59335354afee 299 EMG_filtered[c] = filter_value[c];
PatrickZieverink 0:59335354afee 300 }
PatrickZieverink 0:59335354afee 301 enc_value[c] -= enc_zero[c];
PatrickZieverink 0:59335354afee 302 theta[c] = (float)(enc_value[c])/(float)(8400)*2*PI; //Revoluties Theta 0 en 1 zijn de gemeten waardes hier!
PatrickZieverink 0:59335354afee 303 }
PatrickZieverink 0:59335354afee 304
PatrickZieverink 0:59335354afee 305 if (EMG_high_has_been_calibrated && EMG_low_has_been_calibrated){
PatrickZieverink 0:59335354afee 306 if (EMG_filtered[0] > EMG_filtered[1]){
PatrickZieverink 0:59335354afee 307 EMG_filtered[1] = 0;
PatrickZieverink 0:59335354afee 308 }
PatrickZieverink 0:59335354afee 309 else {
PatrickZieverink 0:59335354afee 310 EMG_filtered[0] = 0;
PatrickZieverink 0:59335354afee 311 }
PatrickZieverink 0:59335354afee 312 }
PatrickZieverink 0:59335354afee 313
PatrickZieverink 0:59335354afee 314 if(button3_pressed) {
PatrickZieverink 0:59335354afee 315 direction_switch = -1.0*direction_switch;
PatrickZieverink 0:59335354afee 316 button3_pressed = false;
PatrickZieverink 0:59335354afee 317 }
PatrickZieverink 0:59335354afee 318
PatrickZieverink 0:59335354afee 319
PatrickZieverink 0:59335354afee 320 theta[1] = theta[1]*(5.05*0.008/2.0/PI)+0.63;
PatrickZieverink 0:59335354afee 321 theta[0] = theta[0]*-1.0;
PatrickZieverink 0:59335354afee 322
PatrickZieverink 0:59335354afee 323 float y = theta[1]*sin(theta[0]); // quick y calculation
PatrickZieverink 0:59335354afee 324
PatrickZieverink 0:59335354afee 325 for(int c = 0; c<2; c++) {
PatrickZieverink 0:59335354afee 326
PatrickZieverink 0:59335354afee 327 speed[c] = schmitt_trigger(EMG_filtered[c]);
PatrickZieverink 0:59335354afee 328 if (speed[c] == -1) {
PatrickZieverink 0:59335354afee 329 speed[c] = past_speed[c];
PatrickZieverink 0:59335354afee 330 }
PatrickZieverink 0:59335354afee 331 past_speed[c] = speed[c];
PatrickZieverink 0:59335354afee 332 if (speed[c] == 0) {
PatrickZieverink 0:59335354afee 333 velocity_desired[c] = 0.00f*direction_switch;
PatrickZieverink 0:59335354afee 334 }
PatrickZieverink 0:59335354afee 335 if (speed[c] == 1) {
PatrickZieverink 0:59335354afee 336 velocity_desired[c] = 0.01f*direction_switch;
PatrickZieverink 0:59335354afee 337 }
PatrickZieverink 0:59335354afee 338 if (speed[c] == 2) {
PatrickZieverink 0:59335354afee 339 velocity_desired[c] = 0.02f*direction_switch;
PatrickZieverink 0:59335354afee 340 }
PatrickZieverink 0:59335354afee 341
PatrickZieverink 0:59335354afee 342 // Joystick beweging, comment dan de EMG zooi!
PatrickZieverink 0:59335354afee 343 /*
PatrickZieverink 0:59335354afee 344 if ( c == 0){
PatrickZieverink 0:59335354afee 345 velocity_desired[c] = (vrx.read()*100-50)/50*0.02;
PatrickZieverink 0:59335354afee 346 if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
PatrickZieverink 0:59335354afee 347 velocity_desired[c] = 0;
PatrickZieverink 0:59335354afee 348 }
PatrickZieverink 0:59335354afee 349 }
PatrickZieverink 0:59335354afee 350 if ( c == 1){
PatrickZieverink 0:59335354afee 351 velocity_desired[c] = (vry.read()*100-50)/50*0.02;
PatrickZieverink 0:59335354afee 352 if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
PatrickZieverink 0:59335354afee 353 velocity_desired[c] = 0;
PatrickZieverink 0:59335354afee 354 }
PatrickZieverink 0:59335354afee 355 }
PatrickZieverink 0:59335354afee 356 */
PatrickZieverink 0:59335354afee 357 }
PatrickZieverink 0:59335354afee 358 }
PatrickZieverink 0:59335354afee 359
PatrickZieverink 0:59335354afee 360
PatrickZieverink 0:59335354afee 361 void motor_controller() //s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure
PatrickZieverink 0:59335354afee 362 {
PatrickZieverink 0:59335354afee 363 float P_action[2];
PatrickZieverink 0:59335354afee 364 float I_action[2];
PatrickZieverink 0:59335354afee 365 float D_action[2];
PatrickZieverink 0:59335354afee 366 float mg;
PatrickZieverink 0:59335354afee 367 float duty_mg;
PatrickZieverink 0:59335354afee 368 float velocity_estimate[2];
PatrickZieverink 0:59335354afee 369 for (int c = 0; c<2; c++) {
PatrickZieverink 0:59335354afee 370
PatrickZieverink 0:59335354afee 371 //P action
PatrickZieverink 0:59335354afee 372 P_action[c] = PID.P[c] * errors[c];
PatrickZieverink 0:59335354afee 373
PatrickZieverink 0:59335354afee 374 //I part
PatrickZieverink 0:59335354afee 375 PID.I_counter[c] += errors[c]*dt;
PatrickZieverink 0:59335354afee 376 if (PID.I_counter[c] > 0.05) {
PatrickZieverink 0:59335354afee 377 PID.I_counter[c] = 0.05;
PatrickZieverink 0:59335354afee 378 }
PatrickZieverink 0:59335354afee 379 if (PID.I_counter[c] < -0.05) {
PatrickZieverink 0:59335354afee 380 PID.I_counter[c] = -0.05;
PatrickZieverink 0:59335354afee 381 }
PatrickZieverink 0:59335354afee 382 I_action[c] = PID.I_counter[c] * PID.I[c];
PatrickZieverink 0:59335354afee 383
PatrickZieverink 0:59335354afee 384 //D part
PatrickZieverink 0:59335354afee 385 velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
PatrickZieverink 0:59335354afee 386 D_action[c] = velocity_estimate[c] * PID.D[c];
PatrickZieverink 0:59335354afee 387
PatrickZieverink 0:59335354afee 388 action[c] = (P_action[c] + I_action[c] + D_action[c])/dt;
PatrickZieverink 0:59335354afee 389 last_error[c] = errors[c];
PatrickZieverink 0:59335354afee 390 }
PatrickZieverink 0:59335354afee 391
PatrickZieverink 0:59335354afee 392 if (theta[0]> 50){
PatrickZieverink 0:59335354afee 393 mg = 0;
PatrickZieverink 0:59335354afee 394 } else { mg = (theta[1]-0.125)*9.81*0.200*cos(theta[0]*1.75); } // zwaartekracht compensatie
PatrickZieverink 0:59335354afee 395 duty_mg = mg/10.0;
PatrickZieverink 0:59335354afee 396 action[0] += duty_mg;
PatrickZieverink 0:59335354afee 397
PatrickZieverink 0:59335354afee 398 for (int c = 0; c<2; c++) {
PatrickZieverink 0:59335354afee 399 actuator.direction[c] = (action[c] > 0); //1 if action is positive, 0 otherwise
PatrickZieverink 0:59335354afee 400 actuator.duty_cycle[c] = fabs(action[c]);
PatrickZieverink 0:59335354afee 401 if (actuator.duty_cycle[c] > 1.0f) {
PatrickZieverink 0:59335354afee 402 actuator.duty_cycle[c] = 1.0f;
PatrickZieverink 0:59335354afee 403 }
PatrickZieverink 0:59335354afee 404 if (actuator.duty_cycle[c] < 0.0f) {
PatrickZieverink 0:59335354afee 405 actuator.duty_cycle[c] = 0.0f;
PatrickZieverink 0:59335354afee 406 }
PatrickZieverink 0:59335354afee 407 }
PatrickZieverink 0:59335354afee 408 }
PatrickZieverink 0:59335354afee 409
PatrickZieverink 0:59335354afee 410 void output()
PatrickZieverink 0:59335354afee 411 {
PatrickZieverink 0:59335354afee 412 for (int c = 0; c <2; c++) {
PatrickZieverink 0:59335354afee 413 if (actuator.default_direction[c] == false) {
PatrickZieverink 0:59335354afee 414 if (actuator.direction[c] == 1) {
PatrickZieverink 0:59335354afee 415 actuator.direction[c] = 0;
PatrickZieverink 0:59335354afee 416 } else {
PatrickZieverink 0:59335354afee 417 actuator.direction[c] = 1;
PatrickZieverink 0:59335354afee 418 }
PatrickZieverink 0:59335354afee 419 } else {
PatrickZieverink 0:59335354afee 420 if (actuator.direction[c] == 1) {
PatrickZieverink 0:59335354afee 421 actuator.direction[c] = 1;
PatrickZieverink 0:59335354afee 422 } else {
PatrickZieverink 0:59335354afee 423 actuator.direction[c] = 0;
PatrickZieverink 0:59335354afee 424 }
PatrickZieverink 0:59335354afee 425 }
PatrickZieverink 0:59335354afee 426 }
PatrickZieverink 0:59335354afee 427
PatrickZieverink 0:59335354afee 428 motor0_direction = actuator.direction[0];
PatrickZieverink 0:59335354afee 429 motor1_direction = actuator.direction[1];
PatrickZieverink 0:59335354afee 430 motor0_pwm.write(actuator.duty_cycle[0]);
PatrickZieverink 0:59335354afee 431 motor1_pwm.write(actuator.duty_cycle[1]);
PatrickZieverink 0:59335354afee 432
PatrickZieverink 0:59335354afee 433 scope.set(0, EMG_filtered[0]);
PatrickZieverink 0:59335354afee 434 scope.set(1, EMG_filtered[1]);
PatrickZieverink 0:59335354afee 435 scope.set(2, actuator.duty_cycle[0]);
PatrickZieverink 0:59335354afee 436 scope.set(3, actuator.duty_cycle[1]);
PatrickZieverink 0:59335354afee 437 scope.set(4, speed[0]);
PatrickZieverink 0:59335354afee 438 scope.set(5, speed[1]);
PatrickZieverink 0:59335354afee 439 /*
PatrickZieverink 0:59335354afee 440 scope.set(0, actuator.duty_cycle[1]);
PatrickZieverink 0:59335354afee 441 scope.set(1, theta[1]);
PatrickZieverink 0:59335354afee 442 scope.set(2, theta_desired[1]);
PatrickZieverink 0:59335354afee 443 scope.set(3, actuator.duty_cycle[0]);
PatrickZieverink 0:59335354afee 444 scope.set(4, theta[0]);
PatrickZieverink 0:59335354afee 445 scope.set(5, errors[0]);
PatrickZieverink 0:59335354afee 446 */
PatrickZieverink 0:59335354afee 447 }
PatrickZieverink 0:59335354afee 448
PatrickZieverink 0:59335354afee 449 void state_machine()
PatrickZieverink 0:59335354afee 450 {
PatrickZieverink 0:59335354afee 451 check_failure(); //check for an error in the last loop before state machine
PatrickZieverink 0:59335354afee 452 //run current state
PatrickZieverink 0:59335354afee 453 switch (state) {
PatrickZieverink 0:59335354afee 454 case s_idle:
PatrickZieverink 0:59335354afee 455 do_nothing();
PatrickZieverink 0:59335354afee 456 break;
PatrickZieverink 0:59335354afee 457 case s_failure:
PatrickZieverink 0:59335354afee 458 failure();
PatrickZieverink 0:59335354afee 459 break;
PatrickZieverink 0:59335354afee 460 case s_cali_EMG_low:
PatrickZieverink 0:59335354afee 461 cali_EMG_low();
PatrickZieverink 0:59335354afee 462 break;
PatrickZieverink 0:59335354afee 463 case s_cali_EMG_high:
PatrickZieverink 0:59335354afee 464 cali_EMG_high();
PatrickZieverink 0:59335354afee 465 break;
PatrickZieverink 0:59335354afee 466 case s_cali_enc:
PatrickZieverink 0:59335354afee 467 cali_enc();
PatrickZieverink 0:59335354afee 468 break;
PatrickZieverink 0:59335354afee 469 case s_moving_magnet_on:
PatrickZieverink 0:59335354afee 470 moving_magnet_on();
PatrickZieverink 0:59335354afee 471 break;
PatrickZieverink 0:59335354afee 472 case s_moving_magnet_off:
PatrickZieverink 0:59335354afee 473 moving_magnet_off();
PatrickZieverink 0:59335354afee 474 break;
PatrickZieverink 0:59335354afee 475 case s_homing:
PatrickZieverink 0:59335354afee 476 homing();
PatrickZieverink 0:59335354afee 477 break;
PatrickZieverink 0:59335354afee 478 }
PatrickZieverink 0:59335354afee 479 }
PatrickZieverink 0:59335354afee 480
PatrickZieverink 0:59335354afee 481 void main_loop()
PatrickZieverink 0:59335354afee 482 {
PatrickZieverink 0:59335354afee 483 measure_signals();
PatrickZieverink 0:59335354afee 484 state_machine();
PatrickZieverink 0:59335354afee 485 motor_controller();
PatrickZieverink 0:59335354afee 486 output();
PatrickZieverink 0:59335354afee 487
PatrickZieverink 0:59335354afee 488 }
PatrickZieverink 0:59335354afee 489
PatrickZieverink 0:59335354afee 490 //Helper functions, not directly called by the main_loop functions or
PatrickZieverink 0:59335354afee 491 //state machines
PatrickZieverink 0:59335354afee 492 void check_failure()
PatrickZieverink 0:59335354afee 493 {
PatrickZieverink 0:59335354afee 494 if (failure_occurred) {
PatrickZieverink 0:59335354afee 495 state = s_failure;
PatrickZieverink 0:59335354afee 496 state_changed = true;
PatrickZieverink 0:59335354afee 497 }
PatrickZieverink 0:59335354afee 498 }
PatrickZieverink 0:59335354afee 499
PatrickZieverink 0:59335354afee 500 void but1_interrupt()
PatrickZieverink 0:59335354afee 501 {
PatrickZieverink 0:59335354afee 502 if(!but2.read() || !but3.read()) {//both buttons are pressed
PatrickZieverink 0:59335354afee 503 failure_occurred = true;
PatrickZieverink 0:59335354afee 504 }
PatrickZieverink 0:59335354afee 505 button1_pressed = true;
PatrickZieverink 0:59335354afee 506 pc.printf("Button 1 pressed!\n\r");
PatrickZieverink 0:59335354afee 507 }
PatrickZieverink 0:59335354afee 508
PatrickZieverink 0:59335354afee 509 void but2_interrupt()
PatrickZieverink 0:59335354afee 510 {
PatrickZieverink 0:59335354afee 511 if(!but1.read() || !but3.read()) {//both buttons are pressed
PatrickZieverink 0:59335354afee 512 failure_occurred = true;
PatrickZieverink 0:59335354afee 513 }
PatrickZieverink 0:59335354afee 514 button2_pressed = true;
PatrickZieverink 0:59335354afee 515 pc.printf("Button 2 pressed!\n\r");
PatrickZieverink 0:59335354afee 516 }
PatrickZieverink 0:59335354afee 517
PatrickZieverink 0:59335354afee 518 void but3_interrupt()
PatrickZieverink 0:59335354afee 519 {
PatrickZieverink 0:59335354afee 520 if(!but1.read() || !but2.read()) {
PatrickZieverink 0:59335354afee 521 failure_occurred = true;
PatrickZieverink 0:59335354afee 522 }
PatrickZieverink 0:59335354afee 523 button3_pressed = true;
PatrickZieverink 0:59335354afee 524
PatrickZieverink 0:59335354afee 525 pc.printf("Button 3 pressed!\n\r");
PatrickZieverink 0:59335354afee 526 }
PatrickZieverink 0:59335354afee 527
PatrickZieverink 0:59335354afee 528 int schmitt_trigger(float i)
PatrickZieverink 0:59335354afee 529 {
PatrickZieverink 0:59335354afee 530 int speed;
PatrickZieverink 0:59335354afee 531 speed = -1; //default value, this means the state should not change
PatrickZieverink 0:59335354afee 532 if (i > 0.000f && i < 0.125f) {
PatrickZieverink 0:59335354afee 533 speed = 0;
PatrickZieverink 0:59335354afee 534 }
PatrickZieverink 0:59335354afee 535 if (i > 0.250f && i < 0.375f) {
PatrickZieverink 0:59335354afee 536 speed = 1;
PatrickZieverink 0:59335354afee 537 }
PatrickZieverink 0:59335354afee 538 if (i > 0.500f && i < 1.000f) {
PatrickZieverink 0:59335354afee 539 speed = 2;
PatrickZieverink 0:59335354afee 540 }
PatrickZieverink 0:59335354afee 541 return speed;
PatrickZieverink 0:59335354afee 542 }
PatrickZieverink 0:59335354afee 543
PatrickZieverink 0:59335354afee 544 int main()
PatrickZieverink 0:59335354afee 545 {
PatrickZieverink 0:59335354afee 546 pc.baud(115200);
PatrickZieverink 0:59335354afee 547 pc.printf("Executing main()... \r\n");
PatrickZieverink 0:59335354afee 548 state = s_idle; // Hij slaat nu dus de calibratie over!
PatrickZieverink 0:59335354afee 549
PatrickZieverink 0:59335354afee 550 motor0_pwm.period(1.0f/160000.0f); // 1/frequency van waarop hij draait
PatrickZieverink 0:59335354afee 551 motor1_pwm.period(1.0f/160000.0f); // 1/frequency van waarop hij draait
PatrickZieverink 0:59335354afee 552
PatrickZieverink 0:59335354afee 553 actuator.direction[0] = 0;
PatrickZieverink 0:59335354afee 554 actuator.direction[1] = 0;
PatrickZieverink 0:59335354afee 555
PatrickZieverink 0:59335354afee 556 actuator.default_direction[0] = true;
PatrickZieverink 0:59335354afee 557 actuator.default_direction[1] = false; // dit is gedubbelcheckt!
PatrickZieverink 0:59335354afee 558
PatrickZieverink 0:59335354afee 559 direction_switch = 1.0;
PatrickZieverink 0:59335354afee 560
PatrickZieverink 0:59335354afee 561 PID.P[0] = 1.75;
PatrickZieverink 0:59335354afee 562 PID.P[1] = 0.5;
PatrickZieverink 0:59335354afee 563 PID.I[0] = 0.0;
PatrickZieverink 0:59335354afee 564 PID.I[1] = 0.0;
PatrickZieverink 0:59335354afee 565 PID.D[0] = 0.005;
PatrickZieverink 0:59335354afee 566 PID.D[1] = 0.0;
PatrickZieverink 0:59335354afee 567 PID.I_counter[0] = 0.0;
PatrickZieverink 0:59335354afee 568 PID.I_counter[1] = 0.0;
PatrickZieverink 0:59335354afee 569
PatrickZieverink 0:59335354afee 570 theta_desired[0] = 0.0;
PatrickZieverink 0:59335354afee 571 theta_desired[1] = 0.63;
PatrickZieverink 0:59335354afee 572
PatrickZieverink 0:59335354afee 573 actuator.magnet = false;
PatrickZieverink 0:59335354afee 574 EMG.max[0] = 0.01;
PatrickZieverink 0:59335354afee 575 EMG.max[1] = 0.01;
PatrickZieverink 0:59335354afee 576
PatrickZieverink 0:59335354afee 577 but1.fall(&but1_interrupt);
PatrickZieverink 0:59335354afee 578 but2.fall(&but2_interrupt);
PatrickZieverink 0:59335354afee 579 but3.fall(&but3_interrupt);
PatrickZieverink 0:59335354afee 580 scope_ticker.attach(&scope, &HIDScope::send, 0.05);
PatrickZieverink 0:59335354afee 581 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
PatrickZieverink 0:59335354afee 582 pc.printf("Main_loop is running\n\r");
PatrickZieverink 0:59335354afee 583 while (true) {
PatrickZieverink 0:59335354afee 584 wait(0.1f);
PatrickZieverink 0:59335354afee 585 }
PatrickZieverink 0:59335354afee 586 }