Casper Maas
fork demo mode 20:58
Dependencies: biquadFilter MODSERIAL QEI mbed
Fork of
StateMachine_EMg_RKI_PID_MOTOR
by 
Casper Maas
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // EMG + KINEMATICS + PID + MOTOR CONTROL 00002 00003 //----------------~INITIATING------------------------- 00004 #include "mbed.h" 00005 00006 // EMG -- DEPENDENCIES 00007 #include <iostream> 00008 #include "BiQuad.h" 00009 #include "BiQuadchains_zelfbeun.h" 00010 #include "MODSERIAL.h" 00011 00012 // KINEMATICS -- DEPENDENCIES 00013 #include "stdio.h" 00014 #define _USE_MATH_DEFINES 00015 #include <math.h> 00016 #define M_PI 3.14159265358979323846 /* pi */ 00017 00018 // PID CONTROLLER -- DEPENDENCIES 00019 #include "BiQuad.h" 00020 #include "QEI.h" 00021 //#include "HIDScope.h" 00022 00023 00024 // GENERAL PIN DEFENITIONS 00025 MODSERIAL pc(USBTX, USBRX); 00026 00027 // EMG -- PIN DEFENITIONS 00028 DigitalOut gpo(D0); 00029 00030 DigitalIn button2(SW3); 00031 DigitalIn button1(SW2); //or SW2 00032 00033 DigitalOut led1(LED_GREEN); 00034 DigitalOut led2(LED_RED); 00035 DigitalOut led3(LED_BLUE); 00036 00037 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample 00038 Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach 00039 Ticker threshold_check_ticker; 00040 Timer t; //timer try out for Astrid 00041 Timer timer_calibration; //timer for EMG calibration 00042 00043 //Input system 00044 AnalogIn emg1(A0); //right biceps 00045 AnalogIn emg2(A1); //right triceps 00046 AnalogIn emg3(A2); //left biceps 00047 AnalogIn emg4(A3); //left triceps 00048 00049 00050 // PID CONTROLLER -- PIN DEFENITIONS 00051 //AnalogIn button3(A4); 00052 //AnalogIn button4(A5); 00053 00054 DigitalOut directionpin1(D7); // motor 1 00055 DigitalOut directionpin2(D4); // motor 2 00056 DigitalOut directionpin3(D13); // motor 3 00057 PwmOut pwmpin1(D6); // motor 1 00058 PwmOut pwmpin2(D5); // motor 2 00059 PwmOut pwmpin3(D12); // motor 3 00060 00061 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING); 00062 QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2 00063 QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING); // motor 3 00064 // HIDScope scope(2); 00065 00066 // PID - TICKERS 00067 Ticker ref_rot; 00068 Ticker show_counts; 00069 Ticker Scope_Data; 00070 00071 //------------------------GLOBALS----------------------------- 00072 // GLOBALS EMG 00073 //Filtered EMG signals from the end of the chains 00074 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; 00075 int i = 0; 00076 00077 //Define doubles for calibration and ticker 00078 double ts = 0.001; //tijdsstap 00079 double calibration_time = 55; //time EMG calibration should take 00080 00081 volatile double temp_highest_emg1 = 0; //highest detected value right biceps 00082 volatile double temp_highest_emg2 = 0; 00083 volatile double temp_highest_emg3 = 0; 00084 volatile double temp_highest_emg4 = 0; 00085 00086 //Doubles for calculation threshold 00087 double biceps_p_t = 0.4; //set threshold at percentage of highest value 00088 double triceps_p_t = 0.5; //set threshold at percentage of highest value 00089 volatile double threshold1; 00090 volatile double threshold2; 00091 volatile double threshold3; 00092 volatile double threshold4; 00093 00094 // thresholdreads bools 00095 int bicepsR; 00096 int tricepsR; 00097 int bicepsL; 00098 int tricepsL; 00099 00100 // VARIABLES ROBOT KINEMATICS 00101 // constants 00102 const float la = 0.256; // lengte actieve arm 00103 const float lp = 0.21; // lengte passieve arm 00104 const float rp = 0.052; // straal van midden end effector tot hoekpunt 00105 const float rm = 0.23; // straal van global midden tot motor 00106 const float a = 0.09; // zijde van de driehoek 00107 const float xas = 0.40; // afstand van motor 1 tot motor 3 00108 const float yas = 0.346; // afstand van xas tot motor 2 00109 const float thetap = 0; // rotatiehoek van de end effector 00110 00111 00112 // motor locatie 00113 const int a1x = 0; //x locatie motor 1 00114 const int a1y = 0; //y locatie motor 1 00115 const float a2x = (0.5)*xas; // x locatie motor 2 00116 const float a2y = yas; // y locatie motor 2 00117 const float a3x = xas; // x locatie motor 3 00118 const int a3y = 0; // y locatie motor 3 00119 00120 // script voor het bepalen van de desired position aan de hand van emg (1/0) 00121 00122 // EMG OUTPUT 00123 int EMGxplus; 00124 int EMGxmin ; 00125 int EMGyplus; 00126 int EMGymin ; 00127 00128 // Dit moet experimenteel geperfectioneerd worden 00129 float tijdstap = 0.005; //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE 00130 float v = 0.1; // snelheid kan wss ook hoger 00131 00132 float px = 0.2; //starting x // BOUNDARIES 00133 float py = 0.155; // starting y // BOUNDARIES 00134 00135 // verschil horizontale as met de actieve arm 00136 float da1 = 1.619685; // verschil a1 hoek en motor 00137 float da2 = -0.609780; 00138 float da3 = 3.372859; 00139 00140 // limits (since no forward kinematics) 00141 float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan 00142 float lowerxlim = 0.10; 00143 float upperylim = 0.225; 00144 float lowerylim = 0.03; //0.03 is goed 00145 00146 // VARIABLES PID CONTROLLER 00147 double PI = M_PI;// CHANGE THIS INTO M_PI 00148 double Kp = 14; //200 , 50 00149 double Ki = 0; //1, 0.5 00150 double Kd = 3; //200, 10 00151 double Ts = 0.1; // Sample time in seconds 00152 double reference_rotation; //define as radians 00153 double motor_position; 00154 bool AlwaysTrue; 00155 00156 //----------------FUNCTIONS-------------------------- 00157 00158 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ 00159 void emgsample() 00160 { 00161 //All EMG signal through Highpass 00162 double emgread1 = emg1.read(); 00163 double emgread2 = emg2.read(); 00164 double emgread3 = emg3.read(); 00165 double emgread4 = emg4.read(); 00166 00167 double emg1_highpassed = highp1.step(emgread1); 00168 double emg2_highpassed = highp2.step(emgread2); 00169 double emg3_highpassed = highp3.step(emgread3); 00170 double emg4_highpassed = highp4.step(emgread4); 00171 00172 //All EMG highpassed through Notch 00173 double emg1_notched = notch1.step(emg1_highpassed); 00174 double emg2_notched = notch2.step(emg2_highpassed); 00175 double emg3_notched = notch3.step(emg3_highpassed); 00176 double emg4_notched = notch4.step(emg4_highpassed); 00177 00178 //All EMG notched rectify 00179 double emg1_abs = abs(emg1_notched); 00180 double emg2_abs = abs(emg2_notched); 00181 double emg3_abs = abs(emg3_notched); 00182 double emg4_abs = abs(emg4_notched); 00183 00184 //All EMG abs into lowpass 00185 emg1_filtered = lowp1.step(emg1_abs); 00186 emg2_filtered = lowp2.step(emg2_abs); 00187 emg3_filtered = lowp3.step(emg3_abs); 00188 emg4_filtered = lowp4.step(emg4_abs); 00189 00190 00191 00192 } 00193 00194 void CalibrationEMG() 00195 { 00196 //static float samples = calibration_time/ts; 00197 while(timer_calibration<55) { 00198 if(timer_calibration>0 && timer_calibration<10) { 00199 led1=!led1; 00200 if(emg1_filtered>temp_highest_emg1) { 00201 temp_highest_emg1= emg1_filtered; 00202 //pc.printf("Temp1 = %f \r\n",temp_highest_emg1); 00203 } 00204 } 00205 if(timer_calibration>10 && timer_calibration<15) { 00206 led1=0; 00207 led2=0; 00208 led3=0; 00209 } 00210 if(timer_calibration>15 && timer_calibration<25) { 00211 led2=!led2; 00212 if(emg2_filtered>temp_highest_emg2) { 00213 temp_highest_emg2= emg2_filtered; 00214 //pc.printf("Temp2 = %f \r\n",temp_highest_emg2); 00215 } 00216 } 00217 if(timer_calibration>25 && timer_calibration<30) { 00218 led1=0; 00219 led2=0; 00220 led3=0; 00221 } 00222 if(timer_calibration>30 && timer_calibration<40) { 00223 led3=!led3; 00224 if(emg3_filtered>temp_highest_emg3) { 00225 temp_highest_emg3= emg3_filtered; 00226 //pc.printf("Temp3 = %f \r\n",temp_highest_emg3); 00227 } 00228 } 00229 if(timer_calibration>40 && timer_calibration<45) { 00230 led1=0; 00231 led2=0; 00232 led3=0; 00233 } 00234 if(timer_calibration>45 && timer_calibration<55) { 00235 led2=!led2; 00236 led3=!led3; 00237 if(emg4_filtered>temp_highest_emg4) { 00238 temp_highest_emg4= emg4_filtered; 00239 //pc.printf("Temp4 = %f \r\n",temp_highest_emg4); 00240 } 00241 } 00242 led1=1; 00243 led2=1; 00244 led3=1; 00245 00246 00247 } 00248 00249 //pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1); 00250 //pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2); 00251 //pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3); 00252 //pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4); 00253 00254 00255 threshold1 = temp_highest_emg1*biceps_p_t; //Right Biceps 00256 threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps 00257 threshold3 = temp_highest_emg3*biceps_p_t; //Left Biceps 00258 threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps 00259 } 00260 00261 //Check if emg_filtered has reached their threshold 00262 void threshold_check() 00263 { 00264 00265 //EMG1 threshold check 00266 if(emg1_filtered>threshold1) { 00267 bicepsR = 1; 00268 } else { 00269 bicepsR= 0; 00270 } 00271 //EMG2 threshold check 00272 if(emg2_filtered>threshold2) { 00273 tricepsR = 1; 00274 } else { 00275 tricepsR= 0; 00276 } 00277 //EMG3 threshold check 00278 if(emg3_filtered>threshold3) { 00279 bicepsL = 1; 00280 } else { 00281 bicepsL= 0; 00282 } 00283 //EMG4 threshold check 00284 if(emg4_filtered>threshold4) { 00285 tricepsL = 1; 00286 } else { 00287 tricepsL= 0; 00288 } 00289 00290 /* 00291 pc.printf("Biceps Right = %i", bicepsR); 00292 pc.printf("Triceps Right = %i",tricepsR); 00293 pc.printf("Biceps Left = %i", bicepsL); 00294 pc.printf("Triceps Left = %i", tricepsL); 00295 */ 00296 00297 00298 } 00299 00300 00301 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~ 00302 00303 // functie x positie 00304 float positionx(int EMGxplus,int EMGxmin) 00305 { 00306 float EMGx = EMGxplus - EMGxmin; 00307 00308 float verplaatsingx = EMGx * tijdstap * v; 00309 float pxnieuw = px + verplaatsingx; 00310 // x limit 00311 if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) { 00312 px = pxnieuw; 00313 } else { 00314 if (pxnieuw >= lowerxlim) { 00315 px = upperxlim; 00316 } else { 00317 px = lowerxlim; 00318 } 00319 } 00320 //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx); 00321 return px; 00322 } 00323 00324 00325 // functie y positie 00326 float positiony(int EMGyplus,int EMGymin) 00327 { 00328 float EMGy = EMGyplus - EMGymin; 00329 00330 float verplaatsingy = EMGy * tijdstap * v; 00331 float pynieuw = py + verplaatsingy; 00332 00333 // y limit 00334 if (pynieuw <= upperylim && pynieuw >= lowerylim) { 00335 py = pynieuw; 00336 } else { 00337 if (pynieuw >= lowerylim) { 00338 py = upperylim; 00339 } else { 00340 py = lowerylim; 00341 } 00342 } 00343 //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy); 00344 return (py); 00345 } 00346 00347 00348 //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~ 00349 // arm 1 --> reference angle motor 1 00350 float hoek1(float px, float py) // input: ref x, ref y 00351 { 00352 float c1x = px - rp * cos(thetap +(M_PI/6)); // x locatie hoekpunt end-effector 00353 float c1y = py - rp*sin(thetap+(M_PI/6)); // y locatie hoekpunt end-effector 00354 float alpha1 = atan2((c1y-a1y),(c1x-a1x)); // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt 00355 float psi1 = acos(( pow(la,2)-pow(lp,2)+pow((c1x-a1x),2)+pow((c1y-a1y),2))/(2*la*sqrt(pow ((c1x-a1x),2)+pow((c1y-a1y),2) ))); //Hoek tussen lijn van motor naar bijbehorende end=effector punt en actieve arm 00356 float a1 = alpha1 + psi1 - da1; //hoek tussen horizontaal en actieve arm 00357 //printf("arm 1 = %f \n\r",a1); 00358 return a1; 00359 } 00360 00361 // arm 2 --> reference angle motor 2 00362 float hoek2(float px, float py) 00363 { 00364 float c2x = px - rp * cos(thetap -(M_PI/2)); 00365 float c2y = py - rp*sin(thetap-(M_PI/2)); 00366 float alpha2 = atan2((c2y-a2y),(c2x-a2x)); 00367 float psi2 = acos(( pow(la,2)-pow(lp,2)+pow((c2x-a2x),2)+pow((c2y-a2y),2))/(2*la*sqrt(pow ((c2x-a2x),2)+pow((c2y-a2y),2) ))); 00368 float a2 = alpha2 + psi2 - da2; 00369 //printf("arm 2 = %f \n\r",a2); 00370 return a2; 00371 } 00372 00373 //arm 3 --> reference angle motor 3 00374 float hoek3(float px, float py) 00375 { 00376 float c3x = px - rp * cos(thetap +(5*M_PI/6)); 00377 float c3y = py - rp*sin(thetap+(5*M_PI/6)); 00378 float alpha3 = atan2((c3y-a3y),(c3x-a3x)); 00379 float psi3 = acos(( pow(la,2)-pow(lp,2)+pow((c3x-a3x),2)+pow((c3y-a3y),2))/(2*la*sqrt(pow ((c3x-a3x),2)+pow((c3y-a3y),2) ))); 00380 float a3 = alpha3 + psi3 - da3; 00381 //printf("arm 3 = %f \n\r",a3); 00382 return a3; 00383 } 00384 00385 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~ 00386 00387 double PID_controller(double error) 00388 { 00389 static double error_integral = 0; 00390 static double error_prev = error; // initialization with this value only done once! 00391 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); 00392 00393 // Proportional part: 00394 double u_k = Kp * error; 00395 00396 // Integral part 00397 error_integral = error_integral + error * Ts; 00398 double u_i = Ki * error_integral; 00399 00400 // Derivative part 00401 double error_derivative = (error - error_prev)/Ts; 00402 double filtered_error_derivative = LowPassFilter.step(error_derivative); 00403 double u_d = Kd * filtered_error_derivative; 00404 error_prev = error; 00405 00406 // Sum all parts and return it 00407 return u_k + u_i + u_d; 00408 } 00409 00410 00411 // DIRECTON AND SPEED CONTROL 00412 void moter_control(double u) 00413 { 00414 00415 directionpin1= u > 0.0f; //eithertrueor false 00416 if (fabs(u)> 0.7f) { 00417 u = 0.7f; 00418 } else { 00419 u= u; 00420 } 00421 pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value 00422 } 00423 00424 void moter2_control(double u) 00425 { 00426 directionpin2= u > 0.0f; //eithertrueor false 00427 if (fabs(u)> 0.7f) { 00428 u = 0.7f; 00429 } else { 00430 u= u; 00431 } 00432 pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value 00433 } 00434 00435 void moter3_control(double u) 00436 { 00437 directionpin3= u > 0.0f; //eithertrueor false 00438 if (fabs(u)> 0.7f) { 00439 u = 0.7f; 00440 } else { 00441 u= u; 00442 } 00443 pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value 00444 } 00445 00446 // CONTROLLING THE MOTOR 00447 void Motor_mover() 00448 { 00449 float px = positionx(bicepsR,bicepsL); // EMG: +x, -x 00450 float py = positiony(tricepsR,tricepsL); // EMG: +y, -y 00451 00452 double motor_position = encoder1.getPulses(); //output in counts 00453 double reference_rotation = hoek1(px, py); 00454 double error = reference_rotation - motor_position*(2*PI)/8400; 00455 double u = PID_controller(error); 00456 moter_control(u); 00457 00458 double motor_position2 = encoder2.getPulses(); //output in counts 00459 double reference_rotation2 = hoek2(px, py); 00460 double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400; 00461 double u_2 = PID_controller(error_2); 00462 moter2_control(u_2); 00463 00464 double motor_position3 = encoder3.getPulses(); //output in counts 00465 double reference_rotation3 = hoek3(px, py); 00466 double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400; 00467 double u_3 = PID_controller(error_3); 00468 moter3_control(u_3); 00469 } 00470 00471 //Activate ticker for Movement state, filtering and Threshold checking 00472 void movement_ticker_activator() 00473 { 00474 sample_ticker.attach(&emgsample, ts); 00475 threshold_check_ticker.attach(&threshold_check, ts); 00476 } 00477 void movement_ticker_deactivator() 00478 { 00479 sample_ticker.detach(); 00480 threshold_check_ticker.detach(); 00481 } 00482 00483 00484 //-------------------- STATE MACHINE -------------------------- 00485 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; 00486 states currentState = MOTORS_OFF; //Chosen startingposition for states 00487 bool stateChanged = true; // Make sure the initialization of first state is executed 00488 00489 void ProcessStateMachine(void) 00490 { 00491 switch (currentState) { 00492 case MOTORS_OFF: 00493 // Actions 00494 if (stateChanged) { 00495 // state initialization: rood 00496 led1 = 1; 00497 led2 = 0; 00498 led3 = 1; 00499 wait (1); 00500 stateChanged = false; 00501 } 00502 00503 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden 00504 if (!button1) { 00505 currentState = CALIBRATION; 00506 stateChanged = true; 00507 } else if (!button2) { 00508 currentState = HOMING ; 00509 stateChanged = true; 00510 } else { 00511 currentState = MOTORS_OFF; 00512 stateChanged = true; 00513 } 00514 00515 break; 00516 00517 case CALIBRATION: 00518 // Actions 00519 if (stateChanged) { 00520 // state initialization: oranje 00521 temp_highest_emg1 = 0; //highest detected value right biceps 00522 temp_highest_emg2 = 0; 00523 temp_highest_emg3 = 0; 00524 temp_highest_emg4 = 0; 00525 00526 timer_calibration.reset(); 00527 timer_calibration.start(); 00528 00529 sample_ticker.attach(&emgsample, ts); 00530 CalibrationEMG(); 00531 sample_ticker.detach(); 00532 timer_calibration.stop(); 00533 00534 00535 stateChanged = false; 00536 } 00537 00538 // State transition logic: automatisch terug naar motors off. 00539 00540 currentState = MOTORS_OFF; 00541 stateChanged = true; 00542 break; 00543 00544 case HOMING: 00545 // Actions 00546 if (stateChanged) { 00547 // state initialization: green 00548 t.reset(); 00549 t.start(); 00550 led1 = 0; 00551 led2 = 1; 00552 led3 = 1; 00553 wait (1); 00554 00555 stateChanged = false; 00556 } 00557 00558 // State transition logic: naar DEMO (button1), naar MOVEMENT(button2) 00559 if (!button1) { 00560 currentState = DEMO; 00561 stateChanged = true; 00562 } else if (!button2) { 00563 currentState = MOVEMENT ; 00564 stateChanged = true; 00565 } else if (t>300) { 00566 t.stop(); 00567 t.reset(); 00568 currentState = MOTORS_OFF ; 00569 stateChanged = true; 00570 } else { 00571 currentState = HOMING ; 00572 stateChanged = true; 00573 } 00574 break; 00575 00576 case DEMO: 00577 // Actions 00578 if (stateChanged) { 00579 // state initialization: light blue 00580 led1 = 0; 00581 led2 = 1; 00582 led3 = 0; 00583 wait (1); 00584 00585 stateChanged = false; 00586 } 00587 00588 // State transition logic: automatisch terug naar HOMING 00589 currentState = HOMING; 00590 stateChanged = true; 00591 break; 00592 00593 case MOVEMENT: 00594 // Actions 00595 if (stateChanged) { 00596 // state initialization: purple 00597 //t.reset(); 00598 //t.start(); 00599 00600 led1 = 1; 00601 led2 = 0; 00602 led3 = 0; 00603 wait(2); 00604 00605 movement_ticker_activator(); 00606 00607 led1 = 0; 00608 led2 = 0; 00609 led3 = 0; 00610 wait(2); 00611 00612 00613 stateChanged = false; 00614 } 00615 00616 // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT 00617 if (!button1) { 00618 movement_ticker_deactivator(); 00619 currentState = CLICK; 00620 stateChanged = true; 00621 } else if (!button2) { 00622 movement_ticker_deactivator(); 00623 currentState = MOTORS_OFF ; 00624 stateChanged = true; 00625 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds 00626 t.start(); 00627 } else if (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) { 00628 t.stop(); 00629 t.reset(); 00630 } 00631 00632 if(t>20) { 00633 movement_ticker_deactivator(); 00634 t.stop(); 00635 t.reset(); 00636 currentState = HOMING ; 00637 stateChanged = true; 00638 } 00639 // here ends the idle checking mode 00640 else{ 00641 //For every muscle a different colour if threshold is passed 00642 if(bicepsR==1) { 00643 led1 = 0; 00644 led2 = 1; 00645 led3 = 1; 00646 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { 00647 led1 = 1; 00648 led2 = 1; 00649 led3 = 1; 00650 } 00651 if(tricepsR==1) { 00652 led1 = 1; 00653 led2 = 0; 00654 led3 = 1; 00655 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { 00656 led1 = 1; 00657 led2 = 1; 00658 led3 = 1; 00659 } 00660 if(bicepsL==1) { 00661 led1 = 1; 00662 led2 = 1; 00663 led3 = 0; 00664 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { 00665 led1 = 1; 00666 led2 = 1; 00667 led3 = 1; 00668 } 00669 if(tricepsL==1) { 00670 led1 = 1; 00671 led2 = 0; 00672 led3 = 0; 00673 } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { 00674 led1 = 1; 00675 led2 = 1; 00676 led3 = 1; 00677 } 00678 //currentState = MOVEMENT ; 00679 //stateChanged = false; 00680 } 00681 00682 break; 00683 00684 case CLICK: 00685 // Actions 00686 if (stateChanged) { 00687 // state initialization: blue 00688 led1 = 1; 00689 led2 = 1; 00690 led3 = 0; 00691 wait (1); 00692 00693 stateChanged = false; 00694 } 00695 00696 // State transition logic: automatisch terug naar MOVEMENT. 00697 00698 currentState = MOVEMENT; 00699 stateChanged = true; 00700 break; 00701 00702 } 00703 } 00704 00705 // --------------------------MAIN-------------------- 00706 00707 00708 int main() 00709 { 00710 00711 //BiQuad Chain add 00712 highp1.add( &highp1_1 ).add( &highp1_2 ); 00713 notch1.add( ¬ch1_1 ).add( ¬ch1_2 ); 00714 lowp1.add( &lowp1_1 ).add(&lowp1_2); 00715 00716 highp2.add( &highp2_1 ).add( &highp2_2 ); 00717 notch2.add( ¬ch2_1 ).add( ¬ch2_2 ); 00718 lowp2.add( &lowp2_1 ).add(&lowp2_2); 00719 00720 highp3.add( &highp3_1 ).add( &highp3_2 ); 00721 notch3.add( ¬ch3_1 ).add( ¬ch3_2 ); 00722 lowp3.add( &lowp3_1 ).add(&lowp3_2); 00723 00724 highp4.add( &highp4_1 ).add( &highp4_2 ); 00725 notch4.add( ¬ch4_1 ).add( ¬ch4_2 ); 00726 lowp4.add( &lowp4_1 ).add(&lowp4_2); 00727 00728 pc.baud(115200); 00729 led1 = 1; 00730 led2 = 1; 00731 led3 = 1; 00732 00733 pwmpin1.period_us(60); // setup motor 00734 ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE 00735 //movement_ticker_activator(); 00736 //emg_sample_ticker(); 00737 while (true) { 00738 ProcessStateMachine(); 00739 00740 /* 00741 if (button2 == false) { 00742 wait(0.01f); 00743 00744 // berekenen positie 00745 float px = positionx(1,0); // EMG: +x, -x 00746 float py = positiony(0,0); // EMG: +y, -y 00747 //printf("positie (%f,%f)\n\r",px,py); 00748 } 00749 00750 if (button1 == false) { 00751 wait(0.01f); 00752 // berekenen positie 00753 float px = positionx(0,1); // EMG: +x, -x 00754 float py = positiony(0,0); // EMG: +y, -y 00755 //printf("positie (%f,%f)\n\r",px,py); 00756 } 00757 /* 00758 if (button3 == false) { 00759 wait(0.01f); 00760 // berekenen positie 00761 float px = positionx(0,0); // EMG: +x, -x 00762 float py = positiony(1,0); // EMG: +y, -y 00763 //printf("positie (%f,%f)\n\r",px,py); 00764 } 00765 00766 if (button4 == false) { 00767 wait(0.01f); 00768 // berekenen positie 00769 float px = positionx(0,0); // EMG: +x, -x 00770 float py = positiony(0,1); // EMG: +y, -y 00771 //printf("positie (%f,%f)\n\r",px,py); 00772 } 00773 */ 00774 } 00775 00776 } 00777 00778 00779 00780 00781 00782
Generated on Mon Jul 18 2022 21:46:42 by
1.7.2