![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
main.cpp
- Committer:
- aschut
- Date:
- 2019-04-20
- Revision:
- 16:d60c845e149e
- Parent:
- 15:26dd70c4a837
File content as of revision 16:d60c845e149e:
//libraries #include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" #include "FastPWM.h" //~~~~~~~~~~~~ Algemeen ~~~~~~~~~~~~~~ DigitalIn button3(SW3); DigitalIn button2(SW2); DigitalIn But2(D12); //Button 2 van het motorshield DigitalIn But1(D13); //Button 1 van het motorshield DigitalOut led1(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); MODSERIAL pc(USBTX, USBRX); Timer t; // State timer, na vijf minuten gaat de state automatisch terug naar een vorige state Timer t2; // Timer voor de grafiek in matlab int count = 0; // Counter voor de grafiek in matlab float counts = 0; // Counter voor de MOVEMENT state, zorgt ervoor dat de tickers niet steeds opnieuw attached worden //~~~~~~~~ Motor waardes ~~~~~~~~~~~~~~~ DigitalOut direction1(D4); // Motor 1 is de rotatie motor FastPWM pwmpin1(D5); FastPWM pwmpin2(D6); // Motor 2 is de motor in de elleboog DigitalOut direction2(D7); volatile float pwm1; //Deze 'pwm' wordt gemaakt met de potmeters, wordt gebruikt voor het testen, maar niet voor de uiteindelijke besturing volatile float pwm2; //Encoder QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING); QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING); double Pulses1; // Afgelezen Encoder pulses double motor_position1; // De berekende hoek double Pulses2; double motor_position2; double error1; // Berekende error tussen de berekende hoek en de referentie hoek double error2; double u1; // PWM signaal dat naar de motor gaat double u2; //Pot meter AnalogIn pot(A5); // De potmeters worden alleen gebruikt bij het testen AnalogIn pot0(A0); float Pot2; float Pot1; //Ticker Ticker Pwm; // Ticker om de motoren te laten bewegen Ticker PotRead; // Ticker voor de Continuous read functie Ticker ServoTick; // Ticker om een PWM signaal met een periode van 20 ms bij een DigitalOut pin te creeëren //Servo DigitalOut myservo1(D8); //Duim DigitalOut myservo2(D11); //Pink tot Middel vinger DigitalOut myservo3(D10); //wijsvinger float Periodlength = 0.02; // de MG996R Servo heeft een PWM periode van 20 ms double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in. double servo_position2; double servo_position3; // Vinger posities float Duim_krom = 0.05; float Duim_recht = 0.85; float MWP_krom = 0.06; float MWP_recht = 0.89; float Wijsvinger_krom = 0.15; float Wijsvinger_recht = 0.93; //~~~~~~~~ EMG waardes ~~~~~~~~~~~~~~~~~ //Input AnalogIn emg1( A1 ); //Duim AnalogIn emg2( A2 ); //Bicep AnalogIn emg3( A3 ); //Dorsaal AnalogIn emg4( A4 ); //Palmair // Tickers Ticker sample_ticker; //ticker voor filteren met 1000Hz Ticker sample_timer; //ticker voor printen van de uiteindelijke EMG waardes met 2 Hz Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 100Hz Timer timer_calibration; //timer voor EMG Kalibratie double ts = 0.001; //tijdsstap float Input1; //Input voor motoren float Input2; //Gefilterde EMG signalen volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie volatile double temp_highest_emg2 = 0; volatile double temp_highest_emg3 = 0; volatile double temp_highest_emg4 = 0; //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden double Duim_p_t = 0.5; double Bicep_p_t = 0.4; double Dorsaal_p_t = 0.6; double Palmair_p_t = 0.5; //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden double Duim_p_tL = 0.5; double Bicep_p_tL = 0.4; double Dorsaal_p_tL = 0.5; double Palmair_p_tL = 0.5; // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren volatile double threshold1; volatile double threshold2; volatile double threshold3; volatile double threshold4; // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren volatile double threshold1L; volatile double threshold2L; volatile double threshold3L; volatile double threshold4L; bool thresholdreach1 = false; // Of de waarde boven de bovenste threshold is geweest bool thresholdreach2 = false; bool thresholdreach3 = false; bool thresholdreach4 = false; // thresholdreads bools bool Duim; bool Bicep; bool Dorsaal; bool Palmair; // filters volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; //EMG1 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp1; BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG2 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp2; BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG3 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp3; BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //EMG4 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren BiQuadChain highp4; BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 ); BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 ); BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 ); //Lowpass first order cutoff 0.4Hz BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 ); //~~~~~~~~ Kinematica ~~~~~~~~~~~~~~~~~~ double stap1; //Stap die bij de referentie hoek wordt opgeteld bij een EMG signaal van 1 double stap2; double KPot; float ElbowReference; float Ellebooghoek1; float Ellebooghoek2; float Ellebooghoek3; float Ellebooghoek4; float PolsReference; float Polshoek1; float Polshoek2; float Polshoek3; float Polshoek4; float Hoeknieuw1; float Hoeknieuw2; //Limiet in graden float lowerlim1 = -900; //Rotatie motor gewricht limiet, gelijk aan 180 graden rotatie beide kanten op float upperlim1 = 900; float lowerlim2 = 0; // Elleboog limiet, van 0 graden (helemaal gebogen) tot 110 graden (helemaal gestrekt) float upperlim2 = 1500; //~~~~~~~~ PID controler waardes ~~~~~~ double Kp1 = 12.5; double Ki1 = 0; double Kd1 = 1; double Kp2 = 20; double Ki2 = 0; double Kd2 = 1; double Ts = 0.0005; // tijdstap voor pwm ticker // ~~~~~~~~~~~~~~~~~~~ EMG functies ~~~~~~~~~~~~~~~~~~ void emgsample() { // EMG signaal lezen double emgread1 = emg1.read(); double emgread2 = emg2.read(); double emgread3 = emg3.read(); double emgread4 = emg4.read(); // Vierde orde highpass filter + notch filter double emg1_highpassed = highp1.step(emgread1); double emg2_highpassed = highp2.step(emgread2); double emg3_highpassed = highp3.step(emgread3); double emg4_highpassed = highp4.step(emgread4); //Rectificatie double emg1_abs = abs(emg1_highpassed); double emg2_abs = abs(emg2_highpassed); double emg3_abs = abs(emg3_highpassed); double emg4_abs = abs(emg4_highpassed); //All EMG abs into lowpass emg1_filtered = lowp1.step(emg1_abs); emg2_filtered = lowp2.step(emg2_abs); emg3_filtered = lowp3.step(emg3_abs); emg4_filtered = lowp4.step(emg4_abs); } //Check of emg_filtered boven de threshold is void threshold_check() { // EMG1 Check if (thresholdreach1 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg1_filtered>threshold1) { Duim = 1; thresholdreach1 = true; } else { Duim= 0; } } else { //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg1_filtered<threshold1L) { Duim = 0; thresholdreach1 = false; } else { Duim= 1; } } // EMG2 Check if (thresholdreach2 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg2_filtered>threshold2) { Bicep = 1; thresholdreach2 = true; } else { Bicep= 0; } } else { //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg2_filtered<threshold2L) { Bicep = 0; thresholdreach2 = false; } else { Bicep= 1; } } // EMG3 Check if (thresholdreach3 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg3_filtered>threshold3) { Dorsaal = 1; thresholdreach3 = true; } else { Dorsaal= 0; } } else { //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg3_filtered<threshold3L) { Dorsaal = 0; thresholdreach3 = false; } else { Dorsaal= 1; } } // EMG4 Check if (thresholdreach4 == false) { //Als emg_filtered nog niet boven de bovenste threshold is geweest //bovenste threshold check if(emg4_filtered>threshold4) { Palmair = 1; thresholdreach4 = true; } else { Palmair= 0; } } else { //Als emg_filtered boven de bovenste threshold is geweest //onderste threshold check if(emg4_filtered<threshold4L) { Palmair = 0; thresholdreach4 = false; } else { Palmair= 1; } } } //~~~~~~~~~ Functies Kinematica ~~~~~~~~~~~~ float Kinematics1(float Bicep) { if (Bicep == 1 && Duim == 0) { //Als alleen Bicep flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog stap1 = 450*Ts; Hoeknieuw1 = PolsReference + stap1; return Hoeknieuw1; } else if (Bicep == 1 && Duim == 1) { //Als Bicep en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omlaag stap1 = 450*Ts; Hoeknieuw1 = PolsReference - stap1; return Hoeknieuw1; } else { return PolsReference; } } float Kinematics2(bool Dorsaal) { if (Dorsaal == 1 && Duim == 0) { //Als alleen Dorsaal flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog stap2 = 300*Ts; Hoeknieuw2 = ElbowReference + stap2; return Hoeknieuw2; } else if (Dorsaal == 1 && Duim == 1) { //Als Dorsaal en Duim flexie wordt afgelezen gaat de waarde van de referentie hoek omhoog stap2 = 300*Ts; Hoeknieuw2 = ElbowReference - stap2; return Hoeknieuw2; } else { return ElbowReference; } } float Limits1(float Polshoek2) { if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten Polshoek3 = Polshoek2; } else { if (Polshoek2 >= upperlim1) { //Boven de limiet Polshoek3 = upperlim1; } else { //Onder de limiet Polshoek3 = lowerlim1; } } return Polshoek3; } float Limits2(float Ellebooghoek2) { if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten Ellebooghoek3 = Ellebooghoek2; } else { if (Ellebooghoek2 >= upperlim2) { //Boven de limiet Ellebooghoek3 = upperlim2; } else { //Onder de limiet Ellebooghoek3 = lowerlim2; } } return Ellebooghoek3; } //~~~~~~~~ PID Controller~~~~~~~~~~~~~~~ double PID_controller1(double error1) { static double error1_integral = 0; static double error1_prev = error1; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: double u_k1 = Kp1 * error1; // Integral part error1_integral = error1_integral + error1 * Ts; double u_i1 = Ki1* error1_integral; // Derivative part double error1_derivative = (error1 - error1_prev)/Ts; double filtered_error1_derivative = LowPassFilter.step(error1_derivative); double u_d1 = Kd1 * filtered_error1_derivative; error1_prev = error1; // Sum all parts and return it return u_k1 + u_i1 + u_d1; } double PID_controller2(double error2) { static double error2_integral = 0; static double error2_prev = error2; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); // Proportional part: double u_k2 = Kp2 * error2; // Integral part error2_integral = error2_integral + error2 * Ts; double u_i2 = Ki2 * error2_integral; // Derivative part double error2_derivative = (error2 - error2_prev)/Ts; double filtered_error2_derivative = LowPassFilter.step(error2_derivative); double u_d2 = Kd2 * filtered_error2_derivative; error2_prev = error2; // Sum all parts and return it return u_k2 + u_i2 + u_d2; } //~~~~~~~~~~~~ Functies Motor ~~~~~~ void moter1_control(double u1) { direction1= u1 > 0.0f; //positief = CW if (fabs(u1)> 0.7f) { u1 = 0.7f; } else { u1= u1; } pwmpin1.write(fabs(u1)) ; } void moter2_control(double u2) { direction2= u2 < 0.0f; //positief = CW if (fabs(u2)> 0.99f) { u2 = 0.99f; } else { u2= u2; } pwmpin2.write(fabs(u2)) ; } void PwmMotor(void) { //Continues Read Pot2 = pot.read(); //Potmeters alleen voor testen Pot1 = pot0.read(); pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1 pwm1 =(Pot1*2)-1; Input1 = Bicep; //Voor testen hier potmeter waarde 'pwm1'invullen Input2 = Palmair; // Reference hoek berekenen, in graden float Polshoek1 = Kinematics1(Input1); float Polshoek4 = Limits1(Polshoek1); PolsReference = Polshoek4; // Reference hoek berekenen, in graden float Ellebooghoek1 = Kinematics2(Input2); float Ellebooghoek4 = Limits2(Ellebooghoek1); ElbowReference = Ellebooghoek4; // Positie motor berekenen, in graden Pulses1 = encoder1.getPulses(); motor_position1 = -(Pulses1/1200)*360; Pulses2 = encoder2.getPulses(); motor_position2 = -(Pulses2/4800)*360; // Error berekenen, naar PID controller en dan naar motor double error1 = PolsReference - motor_position1; double u1 = PID_controller1(error1); moter1_control(u1); double error2 = ElbowReference - motor_position2; double u2 = PID_controller2(error2); moter2_control(u2); } void MotorOn(void) { pwmpin1 = 0; pwmpin2 = 0; Pwm.attach (PwmMotor, Ts); } //~~~~~~~~~~ Servo functies ~~~~~~~~~~~~ void servowait1(void)~ //creeërt een PWM periode van 20 ms met een pulslengte tussen de 500 en 2500 microseconden { double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in myservo1 = true; wait(Pulslength1); myservo1 = false; } void servowait2(void) { double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden myservo2 = true; wait(Pulslength2); myservo2 = false; } void servowait3(void) { double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden myservo3 = true; wait(Pulslength3); myservo3 = false; } void ServoPeriod() { servowait1(); servowait2(); servowait3(); } void ContinuousReader(void) //leest potmeters af { Pot2 = pot.read(); Pot1 = pot0.read(); pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1 pwm1 =(Pot1*2)-1; } void sample() //Print uiteindelijke EMG output { pc.printf("Duim = %i\r\n", Duim); pc.printf("Bicep = %i\r\n",Bicep); pc.printf("Dorsaal = %i\r\n", Dorsaal); pc.printf("Palmair = %i\r\n", Palmair); } // StateMachine enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE}; int f = 1; states currentState = MOTORS_OFF; bool stateChanged = true; // Make sure the initialization of first state is executed void ProcessStateMachine(void) { switch (currentState) { case MOTORS_OFF: // Actions if (stateChanged) { // state initialization: rood led1 = 1; led2 = 0; led3 = 1; wait (1); stateChanged = false; } // State transition logic: Als button 3 word ingedrukt --> calibratie, anders motor uithouden if (!button3) { currentState = CALIBRATION ; stateChanged = true; } else { currentState = MOTORS_OFF; stateChanged = true; } break; case CALIBRATION: // Actions if (stateChanged) { // state initialization: oranje led1 = 0; led2 = 0; led3 = 1; sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz timer_calibration.reset(); timer_calibration.start(); while(timer_calibration<20) { //Binnen 20 seconden alle spieren maximaal aanspannen, hoogste waarde wordt opgeslagen if(timer_calibration>0 && timer_calibration<20) { if(emg1_filtered>temp_highest_emg1) { temp_highest_emg1= emg1_filtered; pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1); } if(emg2_filtered>temp_highest_emg2) { temp_highest_emg2= emg2_filtered; pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2); } if(emg3_filtered>temp_highest_emg3) { temp_highest_emg3= emg3_filtered; pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3); } if(emg4_filtered>temp_highest_emg4) { temp_highest_emg4= emg4_filtered; pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4); } } } pc.printf("threshold calculation\r\n"); threshold1 = temp_highest_emg1*Duim_p_t; //Percentage van de hoogste waarde wordt gebruikt om de bovenste en onderste treshold aan te geven threshold2 = temp_highest_emg2*Bicep_p_t; threshold3 = temp_highest_emg3*Dorsaal_p_t; threshold4 = temp_highest_emg4*Palmair_p_t; threshold1L = temp_highest_emg1*Duim_p_tL; threshold2L = temp_highest_emg2*Bicep_p_tL; threshold3L = temp_highest_emg3*Dorsaal_p_tL; threshold4L = temp_highest_emg4*Palmair_p_tL; stateChanged = false; } sample_ticker.detach(); timer_calibration.stop(); // State transition logic: automatisch naar HOMING 1 currentState = HOMING1; stateChanged = true; break; case HOMING1: //Gebruik de knoppen op het motor shield om het rotatie gewricht naar de beginpositie te brengen // Actions if (stateChanged) { // state initialization: green t.start(); led1 = 0; led2 = 1; led3 = 1; if (!But1) { led1 = 1; float H1 = 0.99f; moter1_control(H1); wait(0.001f); } else if (!But2) { led1 = 1; float H1 = -0.99f; moter1_control(H1); wait(0.001f); } encoder1.reset(); motor_position1 = 0; pwmpin1 = 0; pwmpin2 = 0; ; stateChanged = false; } // State transition logic: naar HOMING 2(button3), na 5 min naar MOTORS_OFF if (!button3) { t.stop(); t.reset(); currentState = HOMING2 ; stateChanged = true; wait(1); } else if (t>300) { t.stop(); t.reset(); currentState = MOTORS_OFF ; stateChanged = true; } else { currentState = HOMING1 ; stateChanged = true; } break; case HOMING2: // gebruik knoppen op het motor shield om het elleboog gewricht naar de begin positie te brengen ( helemaal gebogen) // Actions if (stateChanged) { // state initialization: white t.start(); led1 = 0; led2 = 0; led3 = 0; if (!But1) { led1 = 1; float H2 = 0.98f; moter2_control(H2); wait(0.001f); } else if (!But2) { led1 = 1; float H2 = -0.98f; moter2_control(H2); wait(0.001f); } encoder2.reset(); motor_position2 = 0; pwmpin1 = 0; pwmpin2 = 0; ; stateChanged = false; } // State transition logic: naar DEMO (button2), naar MOVEMENT(button3), na 5 min. automatisch naar MOTORS OFF if (!button2) { currentState = DEMO; stateChanged = true; t.stop(); t.reset(); } else if (!button3) { currentState = MOVEMENT ; stateChanged = true; t.stop(); t.reset(); } else if (t>300) { t.stop(); t.reset(); currentState = MOTORS_OFF ; stateChanged = true; } else { currentState = HOMING2 ; stateChanged = true; } break; case DEMO: // Mogelijkheid om hier je demo routine in te zetten // Actions if (stateChanged) { // state initialization: light blue led1 = 0; led2 = 1; led3 = 0; servo_position1 = 0.5; servo_position2 = 0.5; servo_position3 = 0.5; ServoTick.attach(&ServoPeriod, Periodlength); wait(1.5 ); servo_position1 = 0.9; servo_position2 = 0.9; servo_position3 = 0.9; wait(1.5); servo_position1 = 0.1; servo_position2 = 0.1; servo_position3 = 0.1; wait(1); ServoTick.detach(); wait (1); stateChanged = false; } // State transition logic: automatisch terug naar HOMING2 currentState = HOMING2; stateChanged = true; break; case MOVEMENT: // Hier worden de motoren bestuurd met EMG input // Actions if (stateChanged) { // state initialization: purple t.start(); // na 5 minuten terug naar Homing led1 = 1; led2 = 0; led3 = 0; // Tickers aan if (counts == 0) { pwmpin1 = 0; pwmpin2 = 0; Input1 = pwm1; Input2 = pwm2; Pwm.attach (PwmMotor, Ts); // Motoren gaan aan sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz threshold_check_ticker.attach(&threshold_check, 0.01); //Checkt of threshold overgeschreden wordt sample_timer.attach(&sample, 0.5); // print de EMG waardes servo_position1 = Duim_krom; servo_position2 = MWP_krom; servo_position3 = Wijsvinger_krom; ServoTick.attach(&ServoPeriod, Periodlength); //Servo's gaan aan counts++; wait(1); //Zorgt ervoor dat hij niet doorschiet naar de volgende staat als je te lang de knop indrukt } // Servo positie if (Dorsaal == 1 && Duim == 0) { servo_position1 = Duim_krom; servo_position2 = MWP_krom; servo_position3 = Wijsvinger_krom; led1 = !led1; } if (Dorsaal == 1 && Duim == 1) { servo_position1 = Duim_recht; servo_position2 = MWP_recht; servo_position3 = Wijsvinger_recht; led1 = !led1; } stateChanged = false; } // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3), na vijf min automatisch naar HOMING, anders naar MOVEMENT if (!button2) { currentState = FREEZE; stateChanged = true; threshold_check_ticker.detach(); t.stop(); t.reset(); } else if (!button3) { Pwm.detach (); ServoTick.detach(); sample_ticker.detach(); threshold_check_ticker.detach(); sample_timer.detach(); t.stop(); t.reset(); pwmpin2 = 0; pwmpin1 = 0; counts = 0; currentState = MOTORS_OFF ; stateChanged = true; } else if (t>300) { t.stop(); t.reset(); Pwm.detach (); ServoTick.detach(); sample_ticker.detach(); sample_timer.detach(); threshold_check_ticker.detach(); counts = 0; currentState = HOMING1 ; stateChanged = true; } else { currentState = MOVEMENT ; stateChanged = true; } break; case FREEZE: //Koppelt motoren los van EMG input, de motoren zullen niet meer bewegen // Actions if (stateChanged) { // state initialization: blue Duim = 0; Bicep = 0; Dorsaal = 0; Palmair = 0; led1 = 1; led2 = 1; led3 = 0; wait(1); stateChanged = false; } // State transition logic: button 2 terug naar MOVEMENT. if (!button2) { currentState = MOVEMENT; stateChanged = true; counts = 0; } else { currentState = FREEZE ; stateChanged = false; } break; } } int main() { t2.start(); //Timer voor printen naar matlab int counter = 0; //Counter voor printen naar matlab pwmpin1.period_us(60); // PWM periode voor motoren is 60 microseconden //PotRead.attach(ContinuousReader,Ts); //Gebruikt in test fase voor aflezen potmeters pc.baud(115200); // om naar de PC te printen //BiQuad Chain add (voegt de filters samen tot een reeks) highp1.add( &highp1_1 ).add( &highp1_2 ).add( ¬ch1_1 ); highp2.add( &highp2_1 ).add( &highp2_2 ).add( ¬ch2_1 ); highp3.add( &highp3_1 ).add( &highp3_2 ).add( ¬ch3_1 ); highp4.add( &highp4_1 ).add( &highp4_2 ).add( ¬ch4_1 ); while(true) { led1 = 1; led2 =1; led3 =1; /* //Dit is voor het printen naar matlab if(counter==10) { float tmp = t2.read(); printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2); counter = 0; } counter++; */ ProcessStateMachine(); //Zet de statemachine aan wait(0.001); } }