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@31:9350f76903c3, 2019-10-29 (annotated)

Committer:
PatrickZieverink
Date:
Tue Oct 29 15:50:26 2019 +0000
Revision:
31:9350f76903c3
Parent:
30:af941ee901e5
Child:
32:e22f89879d99
Werkende versie? Gaat alleen verkeerd om met de inverse kinematica doordat de hoek negatief is ofso;

Who changed what in which revision?

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