Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@33:d0621860756c, 2019-10-31 (annotated)

Committer:
Kuipertju
Date:
Thu Oct 31 15:02:44 2019 +0000
Revision:
33:d0621860756c
Parent:
32:2aaa9aae9dc2 
DEMO VERSIE INCLUSIEF ledjes

Who changed what in which revision?

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