Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@31:ffe6eec3dd14, 2019-10-30 (annotated)

Committer:
joostbonekamp
Date:
Wed Oct 30 16:29:06 2019 +0000
Revision:
31:ffe6eec3dd14
Parent:
30:eda0a0591580
Child:
32:2aaa9aae9dc2
added out of bounds logic

Who changed what in which revision?

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