Biorobotics10
/
Samenvoegen6
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: HIDScope MODSERIAL QEI biquadFilter mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //================================================================ Script: Ball-E ================================================================== 00002 // Author: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood 00003 /* This is the script of a EMG measurment moving robot. The purpose of the robot is to amuse people with the disease of Ducenne. 00004 The robot is designed to throw a ball in to a certain chosen pocket. In order to achieve this movement we use a ‘shoulder’ that can turn in the vertical plane 00005 */ 00006 00007 //******************************************** Library DECLARATION ************************************** 00008 #include "mbed.h" 00009 #include "QEI.h" 00010 #include "MODSERIAL.h" 00011 #include "HIDScope.h" 00012 #include "biquadFilter.h" 00013 #include <cmath> 00014 #include <stdio.h> 00015 00016 //******************************************* FUNCTION DECLA ******************************************* 00017 //**************** INPUTS ***************** 00018 AnalogIn EMG1(A0); // Input EMG 00019 AnalogIn EMG2(A1); // Input EMG 00020 //AnalogIn pot(A2); 00021 //AnalogIn pot1(A3); 00022 00023 QEI wheel2 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller 00024 QEI wheel1 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller 00025 00026 //**************** OUTPUTS **************** 00027 DigitalOut red(LED_RED); // LED declared for calibration 00028 DigitalOut green(LED_GREEN); // LED declared for sampling 00029 00030 DigitalOut magnet(D2); 00031 00032 DigitalOut motor2direction(D4); // D4 en D5 zijn motor 2 (op het motorshield) 00033 PwmOut motor2speed(D5); 00034 DigitalOut motor1direction(D7); // D6 en D7 zijn motor 1 (op het motorshield) 00035 PwmOut motor1speed(D6); 00036 00037 //**************** FUNCTIONS ************** 00038 /* HIDScope scope(4); */ // HIDScope declared with 4 scopes 00039 MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc. 00040 00041 //******************************* GLOBAL VARIABLES DECLARATION ************************************ 00042 const int led_on = 0; // Needed for the LEDs to turn on and off 00043 const int led_off = 1; 00044 const int relax = 0; 00045 const int graden = 360; 00046 00047 int games_played = -1; // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken 00048 int games_played1 = 0; 00049 bool flag_calibration = true; 00050 00051 //********* VARIABLES FOR MOTOR CONTROL ******** 00052 const float cw = 1; // The number if: motor1 moves clock wise motor2 moves counterclockwise 00053 const float ccw = 0; // The number if: motor1 moves counterclock wise motor2 moves clockwise 00054 bool flag_s = false; // Var flag_s sensor ticker 00055 bool flag_m = false; // Var flag_m motor ticker 00056 float ain = 0; 00057 00058 //********* VARIABLES FOR CONTROL 1 ************ 00059 volatile bool sample_go; // Ticker EMG function 00060 int Fs = 512; // Sampling frequency 00061 00062 const double low_b1 = 1.480219865318266e-04; //Filter coefficients 00063 const double low_b2 = 2.960439730636533e-04; 00064 const double low_b3 = 1.480219865318266e-04; 00065 const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1 00066 const double low_a3 = 9.658854605688177e-01; 00067 const double high_b1 = 8.047897937631126e-01; 00068 const double high_b2 = -1.609579587526225e+00; 00069 const double high_b3 = 8.047897937631126e-01; 00070 const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1 00071 const double high_a3 = 6.480567348620491e-01; 00072 00073 double u1; double y1; // Declaring the input variables 00074 double u2; double y2; 00075 00076 int calibratie_metingen = 0; 00077 00078 //********* VARIABLES FOR FASE SWITCH ********** 00079 bool begin = true; 00080 int fase = 3; // To switch between different phases and begins with the reset phase 00081 const float fasecontract = 0.7; // The value the EMG signal must be for fase change 00082 float rightbiceps = y2; 00083 float leftbiceps = ain; 00084 int reset = 0; 00085 int button_pressed = 0; 00086 int j = 1; 00087 int N = 200; 00088 bool fase_switch_wait = true; 00089 00090 //********* VARIABLES FOR CONTROL 2 ************ 00091 const float contract = 0.2; // The minimum value for which the motor moves 00092 const float maxleft = -1000; // With this angle the motor should stop moving 00093 const float maxright = 1000; // With this angle the motor should stop moving 00094 const float speed_plate_1 = 0.4; // The speed of the plate in control 2 00095 bool flag_left = true; 00096 bool flag_right = true; 00097 float pos_board = 0; // The position of the board begins at zero 00098 float Encoderread2 = 0; 00099 00100 00101 //********* VARIABLES FOR CONTROL 3 ************ 00102 const float minimum_contract = 0.2; // Certain levels for muscle activity to activate motor 00103 const float medium_contract = 0.5; // "Medium contract muscle" 00104 const float maximum_leftbiceps = 0.8; // "Maximum contract muscle" 00105 const float on = 1.0; 00106 const float off = 0.0; 00107 const float minimum_throw_angle = -5; 00108 const float maximum_throw_angle = 110; 00109 float pos_arm = 0; 00110 int pos_arm1 = 0; 00111 float Encoderread1 = 0; 00112 int switch_rondjes = 2; 00113 int rondjes = 0; 00114 00115 //********* VARIABLES FOR CONTROL 4 ************ 00116 00117 float reset_positie = 0; // Belangrijk 00118 float reset_board = 0; 00119 float speedcompensation; 00120 float speedcompensation2; 00121 int t = 5; // aftellen naar nieuw spel 00122 int switch_reset = 1; 00123 bool arm = false; 00124 bool board1 = false; 00125 00126 00127 // **************************************** Tickers ***************************************** 00128 Ticker Sensor_control; // Adds ticker function for the variable function : Sensors 00129 Ticker Motor_control; 00130 Ticker EMG_Control; 00131 00132 //=============================================================================================== SUB LOOPS ================================================================================================================== 00133 //**************************** CONTROL 1-EMG CALIBRATION *********************************** 00134 00135 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3); // Different objects for different inputs, otherwise the v1 and v2 variables get fucked up 00136 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3); 00137 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3); 00138 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3); 00139 00140 00141 double cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1 00142 double cali_fact2 = 0.9; 00143 double cali_array1[2560] = {}; // array to store values in 00144 double cali_array2[2560] = {}; 00145 00146 void hoog_laag_filter() 00147 { 00148 u1 = EMG1; 00149 u2 = EMG2; 00150 y1 = highpass1.step(u1); 00151 y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass 00152 y1 = fabs(y1); 00153 y2 = fabs(y2); 00154 y1 = lowpass1.step(y1)*cali_fact1; 00155 y2 = lowpass2.step(y2)*cali_fact2; // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output. 00156 } 00157 00158 00159 //======================================= TICKER LOOPS ========================================== 00160 void SENSOR_LOOP() 00161 { 00162 flag_s = true; 00163 } 00164 00165 void MOTOR_LOOP() 00166 { 00167 flag_m = true; 00168 } 00169 00170 void samplego() { // ticker function, set flag to true every sample interval 00171 sample_go = 1; 00172 } 00173 00174 00175 //================================================================================================== HEAD LOOP ================================================================================================================ 00176 int main() 00177 { 00178 00179 pc.baud(115200); 00180 Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION 00181 Motor_control.attach(&MOTOR_LOOP, 0.01); 00182 EMG_Control.attach(&samplego, (float)1/Fs); 00183 00184 pc.printf("===============================================================\n"); 00185 pc.printf(" \t\t\t Ball-E\n"); 00186 pc.printf("In the module Biorobotics on the University of Twente is this script created\n"); 00187 pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n"); 00188 wait(3); 00189 pc.printf("The script will begin with a short calibration\n\n"); 00190 wait(2.5); 00191 pc.printf("===============================================================\n"); 00192 //************************* CONTROL 1-EMG CALIBRATION **************************** 00193 while(1) 00194 { 00195 00196 if(sample_go) 00197 { 00198 red.write(led_off); // Toggles red calibration LED off 00199 green.write(led_on); // Toggles green sampling LED on 00200 hoog_laag_filter(); 00201 sample_go = 0; 00202 } 00203 00204 if (flag_calibration) 00205 { // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that. 00206 calibratie_metingen ++; 00207 green.write(led_off); // Toggles green sampling LED off 00208 red.write(led_on); //Toggles red calibration LED on 00209 cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1 00210 cali_fact2 = 0.9; 00211 double cali_max1 = 0; // declare max y1 00212 double cali_max2 = 0; //declare max y2 00213 pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n"); 00214 wait(2); 00215 pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n"); 00216 wait(0.5); 00217 pc.printf("\t......contract muscles..... \n"); 00218 00219 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1 00220 { 00221 hoog_laag_filter(); 00222 cali_array1[cali_index1] = y1; 00223 wait((float)1/Fs); 00224 } 00225 for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) // Records 5 seconds of y2 00226 { 00227 hoog_laag_filter(); 00228 cali_array2[cali_index2] = y2; 00229 wait((float)1/Fs); 00230 } 00231 for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) // Scales y1 00232 { 00233 if(cali_array1[cali_index3] > cali_max1) 00234 { 00235 cali_max1 = cali_array1[cali_index3]; 00236 } 00237 } 00238 for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) // Scales y2 00239 { 00240 if(cali_array2[cali_index4] > cali_max2) 00241 { 00242 cali_max2 = cali_array2[cali_index4]; 00243 } 00244 } 00245 cali_fact1 = (double)1/cali_max1; // Calibration factor for y1 00246 cali_fact2 = (double)1/cali_max2; // Calibration factor for y2 00247 00248 // Toggles green sampling LED off 00249 red.write(led_off); 00250 green.write(led_on); 00251 pc.printf(" \t....... Calibration has been completed!\n"); 00252 wait(0.2); 00253 green.write(led_off); 00254 wait(0.2); 00255 green.write(led_on); 00256 wait(0.2); 00257 green.write(led_off); 00258 wait(0.2); 00259 green.write(led_on); 00260 wait(4); 00261 pc.printf("Beginning with Ball-E board settings\n"); 00262 green.write(led_off); 00263 wait(2); 00264 y1 = 0; 00265 y2 = 0; 00266 00267 j = 1; // Wachten van fase switch initialiseren 00268 fase_switch_wait = true; 00269 } 00270 00271 00272 //************************* CONTROL MOTOR **************************************** 00273 00274 00275 00276 if (flag_s) 00277 { 00278 Encoderread1 = wheel1.getPulses(); 00279 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm 00280 00281 Encoderread2 = wheel2.getPulses(); 00282 pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board 00283 flag_calibration = false; 00284 } 00285 //************************* FASE SWITCH ****************************************** 00286 //******************** Fase determination ************* 00287 if (fase_switch_wait == true) 00288 { 00289 j++; 00290 wait(0.01); // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen. 00291 pc.printf("waarde j = %i \n",j); 00292 } 00293 00294 if( j > N){ 00295 fase_switch_wait = false; 00296 switch(fase) 00297 { 00298 00299 //******************* Fase 1 ************************** 00300 case(1): 00301 rondjes = 0; 00302 if (y1> contract) 00303 { 00304 flag_right = false; 00305 flag_left = true; 00306 } 00307 00308 if (y2 > contract) 00309 { 00310 flag_right = true; 00311 flag_left = false; 00312 } 00313 00314 if (pos_board <= maxleft) 00315 { 00316 flag_left = false; 00317 motor2speed.write(relax); 00318 } 00319 00320 if (pos_board >= maxright) 00321 { 00322 flag_right = false; 00323 motor2speed.write(relax); 00324 } 00325 00326 if (flag_left == true) 00327 { 00328 if (y1> contract) 00329 { 00330 motor2direction.write(ccw); 00331 motor2speed.write(speed_plate_1); 00332 } 00333 else 00334 { 00335 motor2speed.write(relax); 00336 } 00337 } 00338 00339 if (flag_right == true) 00340 { 00341 if (y2 > contract) 00342 { 00343 motor2direction.write(cw); 00344 motor2speed.write(speed_plate_1); 00345 } 00346 else 00347 { 00348 motor2speed.write(relax); 00349 } 00350 } 00351 pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2); 00352 if (y1> fasecontract && y2 > fasecontract) 00353 { 00354 motor2speed.write(relax); 00355 fase = 2; 00356 fase_switch_wait = true; 00357 j = 0; 00358 } 00359 break; 00360 //******************* Fase 2 ************************** 00361 case(2): 00362 motor1direction.write(cw); 00363 pos_arm1 = (pos_arm - (rondjes * 360)); 00364 00365 switch(switch_rondjes){ 00366 case(1): 00367 rondjes ++; 00368 switch_rondjes = 2; 00369 break; 00370 case(2): 00371 if(pos_arm1>360 & 368<pos_arm1) 00372 { 00373 switch_rondjes = 1; 00374 } 00375 break; 00376 } 00377 00378 00379 if (y2 > minimum_contract & y2 < medium_contract) 00380 { 00381 motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract)); 00382 } 00383 if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief 00384 { 00385 motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract)); 00386 } 00387 if (y2 < minimum_contract) // Lager dan drempel = Rust 00388 { 00389 motor1speed.write(relax); 00390 } 00391 if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle) 00392 { 00393 if (y1> maximum_leftbiceps) 00394 { 00395 magnet.write(off); 00396 motor1speed.write(relax); 00397 fase = 3; 00398 pc.printf("Van fase 2 naar fase 3\n"); 00399 wait(2); 00400 j = 0; 00401 fase_switch_wait = true; 00402 } 00403 } 00404 pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2); 00405 break; 00406 //********************************************* Fase 3 ********************************************** 00407 case(3): 00408 00409 switch(switch_reset) 00410 { 00411 case(1): 00412 if(pos_arm < reset_positie) //ene kant op draaien 00413 { 00414 motor1direction.write(cw); 00415 speedcompensation2 = ((reset_positie - pos_arm)/900.0 + (0.1)); 00416 motor1speed.write(speedcompensation2); 00417 } 00418 if(pos_arm > reset_positie) //andere kant op 00419 { 00420 motor1direction.write(ccw); 00421 speedcompensation2 = ((pos_arm - reset_positie)/500.0 + (0.1)); 00422 motor1speed.write(speedcompensation2); 00423 } 00424 if(pos_arm < reset_positie+5 && pos_arm > reset_positie-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn 00425 { 00426 motor1speed.write(0); 00427 arm = true; 00428 switch_reset = 2; 00429 } 00430 pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation); 00431 wait(0.0001); 00432 break; 00433 00434 case(2): 00435 pc.printf("\t switch magneet automatisch \n"); 00436 wait(1); 00437 magnet.write(on); 00438 wait(0.5); 00439 switch_reset = 3; 00440 break; 00441 00442 case(3): 00443 if(pos_board < reset_board) 00444 { 00445 motor2direction.write(cw); 00446 speedcompensation = ((reset_board - pos_board)/500.0 + (0.1)); 00447 motor2speed.write(speedcompensation); 00448 } 00449 00450 if(pos_board > reset_board) 00451 { 00452 motor2direction.write(ccw); 00453 speedcompensation = ((pos_board - reset_board)/500.0 + (0.1)); 00454 motor2speed.write(speedcompensation); 00455 } 00456 if(pos_board < reset_board+5 && pos_board > reset_board-5) 00457 { 00458 motor2speed.write(0); 00459 board1 = true; 00460 } 00461 if(board1 == true) 00462 { 00463 red=0; 00464 pc.printf("fase switch naar 1\n\n"); 00465 board1 = false; 00466 arm = false; 00467 // flag_calibration = true; !!!!! Moet naar gekeken worden of we elke spel willen calibreren 00468 wait(2); 00469 games_played ++; 00470 games_played1 = games_played - (3*calibratie_metingen - 3); 00471 pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1); 00472 00473 if(games_played1 == 3){ 00474 00475 flag_calibration = true; 00476 while(t){ 00477 pc.printf("\tCalibratie begint over %i \n",t); 00478 t--; 00479 wait(1); 00480 } 00481 } 00482 while(t){ 00483 pc.printf("\tNieuw spel begint over %i \n",t); 00484 t--; 00485 wait(1); 00486 } 00487 00488 fase = 1; // Terug naar fase 1 00489 switch_reset = 1; // De switch op orginele locatie zetten 00490 t = 5; 00491 00492 } 00493 00494 pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation); 00495 wait(0.0001); 00496 break; 00497 } 00498 break; 00499 //=================================================== STOP SCRIPT ============================================================ 00500 } 00501 } 00502 } 00503 }
Generated on Sat Jul 16 2022 01:28:42 by
1.7.2