23:00
Dependencies: biquadFilter MODSERIAL QEI Servo mbed
Fork of StateMachine_EMg_RKI_PID_MOTOR_DEMO_CLICK by
main.cpp
- Committer:
- gastongab
- Date:
- 2018-11-01
- Revision:
- 16:438b330f5312
- Parent:
- 15:6ad7abc5c691
File content as of revision 16:438b330f5312:
// EMG + KINEMATICS + PID + MOTOR CONTROL //----------------~INITIATING------------------------- #include "mbed.h" // EMG -- DEPENDENCIES #include <iostream> #include "BiQuad.h" #include "BiQuadchains_zelfbeun.h" #include "MODSERIAL.h" // KINEMATICS -- DEPENDENCIES #include "stdio.h" #define _USE_MATH_DEFINES #include <math.h> #define M_PI 3.14159265358979323846 /* pi */ // PID CONTROLLER -- DEPENDENCIES #include "BiQuad.h" #include "QEI.h" //#include "HIDScope.h" //Clicker servo library #include "Servo.h" Servo myservo(A5); // GENERAL PIN DEFENITIONS MODSERIAL pc(USBTX, USBRX); // EMG -- PIN DEFENITIONS DigitalOut gpo(D0); DigitalIn button2(SW3); DigitalIn button1(SW2); //or SW2 DigitalOut led1(LED_GREEN); DigitalOut led2(LED_RED); DigitalOut led3(LED_BLUE); //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach Ticker threshold_check_ticker; Timer t; //timer try out for Astrid Timer timer_calibration; //timer for EMG calibration //Input system AnalogIn emg1(A0); //right biceps AnalogIn emg2(A1); //right triceps AnalogIn emg3(A2); //left biceps AnalogIn emg4(A3); //left triceps // PID CONTROLLER -- PIN DEFENITIONS //AnalogIn button3(A4); //AnalogIn button4(A5); DigitalOut directionpin1(D7); // motor 1 DigitalOut directionpin2(D4); // motor 2 DigitalOut directionpin3(D13); // motor 3 PwmOut pwmpin1(D6); // motor 1 PwmOut pwmpin2(D5); // motor 2 PwmOut pwmpin3(D12); // motor 3 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING); QEI encoder2 (D11, D10, NC, 8400, QEI::X4_ENCODING); // motor 2 QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING); // motor 3 // HIDScope scope(2); // PID - TICKERS Ticker ref_rot; Ticker show_counts; Ticker Scope_Data; //------------------------GLOBALS----------------------------- // GLOBALS EMG //Filtered EMG signals from the end of the chains volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered; int i = 0; //Define doubles for calibration and ticker double ts = 0.001; //tijdsstap double calibration_time = 55; //time EMG calibration should take volatile double temp_highest_emg1 = 0; //highest detected value right biceps volatile double temp_highest_emg2 = 0; volatile double temp_highest_emg3 = 0; volatile double temp_highest_emg4 = 0; //Doubles for calculation threshold double biceps_p_t = 0.4; //set threshold at percentage of highest value double triceps_p_t = 0.5; //set threshold at percentage of highest value volatile double threshold1; volatile double threshold2; volatile double threshold3; volatile double threshold4; // thresholdreads bools int bicepsR; int tricepsR; int bicepsL; int tricepsL; // VARIABLES ROBOT KINEMATICS // constants const float la = 0.256; // lengte actieve arm const float lp = 0.21; // lengte passieve arm const float rp = 0.052; // straal van midden end effector tot hoekpunt const float rm = 0.23; // straal van global midden tot motor const float a = 0.09; // zijde van de driehoek const float xas = 0.40; // afstand van motor 1 tot motor 3 const float yas = 0.346; // afstand van xas tot motor 2 const float thetap = 0; // rotatiehoek van de end effector // motor locatie const int a1x = 0; //x locatie motor 1 const int a1y = 0; //y locatie motor 1 const float a2x = (0.5)*xas; // x locatie motor 2 const float a2y = yas; // y locatie motor 2 const float a3x = xas; // x locatie motor 3 const int a3y = 0; // y locatie motor 3 // script voor het bepalen van de desired position aan de hand van emg (1/0) // EMG OUTPUT int EMGxplus; int EMGxmin ; int EMGyplus; int EMGymin ; // Dit moet experimenteel geperfectioneerd worden float tijdstap = 0.001; //nu wss heel langzaam, kan miss omhoog KEER V GEEFT VERANDERING IN POSITIE float v = 0.1; // snelheid kan wss ook hoger float px = 0.2; //starting x // BOUNDARIES float py = 0.155; // starting y // BOUNDARIES // verschil horizontale as met de actieve arm float da1 = 1.619685; // verschil a1 hoek en motor float da2 = -0.609780; float da3 = 3.372859; // limits (since no forward kinematics) float upperxlim = 0.275; //36, 0.04, 0.315, -0.085niet helemaal naar requierments ff kijken of ie groter kan float lowerxlim = 0.10; float upperylim = 0.225; float lowerylim = 0.03; //0.03 is goed // VARIABLES PID CONTROLLER double PI = M_PI;// CHANGE THIS INTO M_PI double Kp = 14; //200 , 50 double Ki = 0; //1, 0.5 double Kd = 3; //200, 10 double Ts = 0.1; // Sample time in seconds double reference_rotation; //define as radians double motor_position; bool AlwaysTrue; //----------------FUNCTIONS-------------------------- // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~ void emgsample() { //All EMG signal through Highpass double emgread1 = emg1.read(); double emgread2 = emg2.read(); double emgread3 = emg3.read(); double emgread4 = emg4.read(); double emg1_highpassed = highp1.step(emgread1); double emg2_highpassed = highp2.step(emgread2); double emg3_highpassed = highp3.step(emgread3); double emg4_highpassed = highp4.step(emgread4); //All EMG highpassed through Notch double emg1_notched = notch1.step(emg1_highpassed); double emg2_notched = notch2.step(emg2_highpassed); double emg3_notched = notch3.step(emg3_highpassed); double emg4_notched = notch4.step(emg4_highpassed); //All EMG notched rectify double emg1_abs = abs(emg1_notched); double emg2_abs = abs(emg2_notched); double emg3_abs = abs(emg3_notched); double emg4_abs = abs(emg4_notched); //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); } void CalibrationEMG() { //static float samples = calibration_time/ts; while(timer_calibration<55) { if(timer_calibration>0 && timer_calibration<10) { led1=!led1; if(emg1_filtered>temp_highest_emg1) { temp_highest_emg1= emg1_filtered; //pc.printf("Temp1 = %f \r\n",temp_highest_emg1); } } if(timer_calibration>10 && timer_calibration<15) { led1=0; led2=0; led3=0; } if(timer_calibration>15 && timer_calibration<25) { led2=!led2; if(emg2_filtered>temp_highest_emg2) { temp_highest_emg2= emg2_filtered; //pc.printf("Temp2 = %f \r\n",temp_highest_emg2); } } if(timer_calibration>25 && timer_calibration<30) { led1=0; led2=0; led3=0; } if(timer_calibration>30 && timer_calibration<40) { led3=!led3; if(emg3_filtered>temp_highest_emg3) { temp_highest_emg3= emg3_filtered; //pc.printf("Temp3 = %f \r\n",temp_highest_emg3); } } if(timer_calibration>40 && timer_calibration<45) { led1=0; led2=0; led3=0; } if(timer_calibration>45 && timer_calibration<55) { led2=!led2; led3=!led3; if(emg4_filtered>temp_highest_emg4) { temp_highest_emg4= emg4_filtered; //pc.printf("Temp4 = %f \r\n",temp_highest_emg4); } } led1=1; led2=1; led3=1; } //pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1); //pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2); //pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3); //pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4); threshold1 = temp_highest_emg1*biceps_p_t; //Right Biceps threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps threshold3 = temp_highest_emg3*biceps_p_t; //Left Biceps threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps } //Check if emg_filtered has reached their threshold void threshold_check() { //EMG1 threshold check if(emg1_filtered>threshold1) { bicepsR = 1; } else { bicepsR= 0; } //EMG2 threshold check if(emg2_filtered>threshold2) { tricepsR = 1; } else { tricepsR= 0; } //EMG3 threshold check if(emg3_filtered>threshold3) { bicepsL = 1; } else { bicepsL= 0; } //EMG4 threshold check if(emg4_filtered>threshold4) { tricepsL = 1; } else { tricepsL= 0; } /* pc.printf("Biceps Right = %i", bicepsR); pc.printf("Triceps Right = %i",tricepsR); pc.printf("Biceps Left = %i", bicepsL); pc.printf("Triceps Left = %i", tricepsL); */ } //-----------------------------DEMO PART--------------------------------------- // DEMO COORDINATES float px1 = 0.2; float py1 = 0.15; float px2 = 0.15; float py2 =0.15; float px3 = 0.25; float py3 = 0.15; float px5 = 0.2; float py5 = 0.2; float px6 = 0.2; float py6 = 0.1; void demomodus() { while(t<48) { if(t>=0 && t<4) { px = px1; py = py1; } else if(t>=4 && t<8) { px = px2; py = py2; } else if(t>=8 && t<12) { px = px3; py = py3; } else if(t>=12 && t<16) { px = px1; py = py1; } else if(t>=16 & t<20) { px = px5; py = py5; } else if(t>=20 && t<24) { px = px6; py = py6; } else if(t>=24 && t<28) { px = px1; py = py1; } else if(t>=28 && t<32) { px = px3; py = py3; } else if(t>=32 && t<36) { px = px5; py = py5; } else if(t>=36 && t<40) { px = px2; py = py2; } else if(t>=40 && t<44) { px = px6; py = py6; } else if(t>=44 && t<48) { px = px1; py = py1; } } } //----------------------------------------------------------------------------- // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~ // functie x positie float positionx(int EMGxplus,int EMGxmin) { float EMGx = EMGxplus - EMGxmin; float verplaatsingx = EMGx * tijdstap * v; float pxnieuw = px + verplaatsingx; // x limit if (pxnieuw <= upperxlim && pxnieuw >= lowerxlim) { px = pxnieuw; } else { if (pxnieuw >= lowerxlim) { px = upperxlim; } else { px = lowerxlim; } } //printf("X eindpunt (%f) en verplaatsing: (%f)\n\r",px,verplaatsingx); return px; } // functie y positie float positiony(int EMGyplus,int EMGymin) { float EMGy = EMGyplus - EMGymin; float verplaatsingy = EMGy * tijdstap * v; float pynieuw = py + verplaatsingy; // y limit if (pynieuw <= upperylim && pynieuw >= lowerylim) { py = pynieuw; } else { if (pynieuw >= lowerylim) { py = upperylim; } else { py = lowerylim; } } //printf("Y eindpunt (%f) en verplaatsing: (%f) \n\r",py,verplaatsingy); return (py); } //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~ // arm 1 --> reference angle motor 1 float hoek1(float px, float py) // input: ref x, ref y { float c1x = px - rp * cos(thetap +(M_PI/6)); // x locatie hoekpunt end-effector float c1y = py - rp*sin(thetap+(M_PI/6)); // y locatie hoekpunt end-effector float alpha1 = atan2((c1y-a1y),(c1x-a1x)); // hoek tussen horizontaal en lijn van motor naar bijbehorende end-effector punt 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 float a1 = alpha1 + psi1 - da1; //hoek tussen horizontaal en actieve arm //printf("arm 1 = %f \n\r",a1); return a1; } // arm 2 --> reference angle motor 2 float hoek2(float px, float py) { float c2x = px - rp * cos(thetap -(M_PI/2)); float c2y = py - rp*sin(thetap-(M_PI/2)); float alpha2 = atan2((c2y-a2y),(c2x-a2x)); 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) ))); float a2 = alpha2 + psi2 - da2; //printf("arm 2 = %f \n\r",a2); return a2; } //arm 3 --> reference angle motor 3 float hoek3(float px, float py) { float c3x = px - rp * cos(thetap +(5*M_PI/6)); float c3y = py - rp*sin(thetap+(5*M_PI/6)); float alpha3 = atan2((c3y-a3y),(c3x-a3x)); 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) ))); float a3 = alpha3 + psi3 - da3; //printf("arm 3 = %f \n\r",a3); return a3; } // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~ double PID_controller(double error) { static double error_integral = 0; static double error_prev = error; // initialization with this value only done once! static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: double u_k = Kp * error; // Integral part error_integral = error_integral + error * Ts; double u_i = Ki * error_integral; // Derivative part double error_derivative = (error - error_prev)/Ts; double filtered_error_derivative = LowPassFilter.step(error_derivative); double u_d = Kd * filtered_error_derivative; error_prev = error; // Sum all parts and return it return u_k + u_i + u_d; } // DIRECTON AND SPEED CONTROL void moter_control(double u) { directionpin1= u > 0.0f; //eithertrueor false if (fabs(u)> 0.7f) { u = 0.7f; } else { u= u; } pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter2_control(double u) { directionpin2= u > 0.0f; //eithertrueor false if (fabs(u)> 0.7f) { u = 0.7f; } else { u= u; } pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } void moter3_control(double u) { directionpin3= u > 0.0f; //eithertrueor false if (fabs(u)> 0.7f) { u = 0.7f; } else { u= u; } pwmpin3 = fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value } // CONTROLLING THE MOTOR void Motor_mover() { float px = positionx(bicepsR,bicepsL); // EMG: +x, -x float py = positiony(tricepsR,tricepsL); // EMG: +y, -y double motor_position = encoder1.getPulses(); //output in counts double reference_rotation = hoek1(px, py); double error = reference_rotation - motor_position*(2*PI)/8400; double u = PID_controller(error); moter_control(u); double motor_position2 = encoder2.getPulses(); //output in counts double reference_rotation2 = hoek2(px, py); double error_2 = reference_rotation2 - motor_position2*(2*PI)/8400; double u_2 = PID_controller(error_2); moter2_control(u_2); double motor_position3 = encoder3.getPulses(); //output in counts double reference_rotation3 = hoek3(px, py); double error_3 = reference_rotation3 - motor_position3*(2*PI)/8400; double u_3 = PID_controller(error_3); moter3_control(u_3); } //Activate ticker for Movement state, filtering and Threshold checking void movement_ticker_activator() { sample_ticker.attach(&emgsample, ts); threshold_check_ticker.attach(&threshold_check, ts); } void movement_ticker_deactivator() { sample_ticker.detach(); threshold_check_ticker.detach(); } //-------------------- STATE MACHINE -------------------------- enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK}; states currentState = MOTORS_OFF; //Chosen startingposition for states 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 1 word ingedrukt --> calibratie, anders motor uithouden if (!button1) { currentState = CALIBRATION; stateChanged = true; } else if (!button2) { currentState = HOMING ; stateChanged = true; } else { currentState = MOTORS_OFF; stateChanged = true; } break; case CALIBRATION: // Actions if (stateChanged) { // state initialization: oranje temp_highest_emg1 = 0; //highest detected value right biceps temp_highest_emg2 = 0; temp_highest_emg3 = 0; temp_highest_emg4 = 0; timer_calibration.reset(); timer_calibration.start(); sample_ticker.attach(&emgsample, ts); CalibrationEMG(); sample_ticker.detach(); timer_calibration.stop(); stateChanged = false; } // State transition logic: automatisch terug naar motors off. currentState = MOTORS_OFF; stateChanged = true; break; case HOMING: // Actions if (stateChanged) { // state initialization: green t.reset(); t.start(); led1 = 0; led2 = 1; led3 = 1; wait (1); stateChanged = false; } // State transition logic: naar DEMO (button1), naar MOVEMENT(button2) if (!button1) { currentState = DEMO; stateChanged = true; } else if (!button2) { currentState = MOVEMENT ; stateChanged = true; } else if (t>300) { t.stop(); t.reset(); currentState = MOTORS_OFF ; stateChanged = true; } else { currentState = HOMING ; stateChanged = true; } break; case DEMO: // Actions if (stateChanged) { // state initialization: light blue led1 = 0; led2 = 1; led3 = 0; wait(1); t.reset(); t.start(); demomodus(); t.stop(); stateChanged = false; } // State transition logic: automatisch terug naar HOMING currentState = HOMING; stateChanged = true; break; case MOVEMENT: // Actions if (stateChanged) { // state initialization: purple //t.reset(); //t.start(); led1 = 1; led2 = 0; led3 = 0; wait(0.5); movement_ticker_activator(); led1 = 0; led2 = 0; led3 = 0; wait(0.5); stateChanged = false; } // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT if (!button1) { movement_ticker_deactivator(); currentState = CLICK; stateChanged = true; } else if (!button2) { movement_ticker_deactivator(); currentState = MOTORS_OFF ; stateChanged = true; } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds t.start(); } else if (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) { t.stop(); t.reset(); } if(t>20) { movement_ticker_deactivator(); t.stop(); t.reset(); currentState = HOMING ; stateChanged = true; } // here ends the idle checking mode else { //For every muscle a different colour if threshold is passed if(bicepsR==1) { led1 = 0; led2 = 1; led3 = 1; } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { led1 = 1; led2 = 1; led3 = 1; } if(tricepsR==1) { led1 = 1; led2 = 0; led3 = 1; } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { led1 = 1; led2 = 1; led3 = 1; } if(bicepsL==1) { led1 = 1; led2 = 1; led3 = 0; } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { led1 = 1; led2 = 1; led3 = 1; } if(tricepsL==1) { led1 = 1; led2 = 0; led3 = 0; } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) { led1 = 1; led2 = 1; led3 = 1; } //currentState = MOVEMENT ; //stateChanged = false; } break; case CLICK: // Actions if (stateChanged) { // state initialization: blue led1 = 1; led2 = 1; led3 = 0; wait(1); for(float p=1; p>0; p -= 0.1) { myservo = p; wait(0.1); } stateChanged = false; } // State transition logic: automatisch terug naar MOVEMENT. currentState = MOVEMENT; stateChanged = true; break; } } // --------------------------MAIN-------------------- int main() { //BiQuad Chain add highp1.add( &highp1_1 ).add( &highp1_2 ); notch1.add( ¬ch1_1 ).add( ¬ch1_2 ); lowp1.add( &lowp1_1 ).add(&lowp1_2); highp2.add( &highp2_1 ).add( &highp2_2 ); notch2.add( ¬ch2_1 ).add( ¬ch2_2 ); lowp2.add( &lowp2_1 ).add(&lowp2_2); highp3.add( &highp3_1 ).add( &highp3_2 ); notch3.add( ¬ch3_1 ).add( ¬ch3_2 ); lowp3.add( &lowp3_1 ).add(&lowp3_2); highp4.add( &highp4_1 ).add( &highp4_2 ); notch4.add( ¬ch4_1 ).add( ¬ch4_2 ); lowp4.add( &lowp4_1 ).add(&lowp4_2); pc.baud(115200); led1 = 1; led2 = 1; led3 = 1; pwmpin1.period_us(60); // setup motor ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE //movement_ticker_activator(); //emg_sample_ticker(); while (true) { ProcessStateMachine(); /* if (button2 == false) { wait(0.01f); // berekenen positie float px = positionx(1,0); // EMG: +x, -x float py = positiony(0,0); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } if (button1 == false) { wait(0.01f); // berekenen positie float px = positionx(0,1); // EMG: +x, -x float py = positiony(0,0); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } /* if (button3 == false) { wait(0.01f); // berekenen positie float px = positionx(0,0); // EMG: +x, -x float py = positiony(1,0); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } if (button4 == false) { wait(0.01f); // berekenen positie float px = positionx(0,0); // EMG: +x, -x float py = positiony(0,1); // EMG: +y, -y //printf("positie (%f,%f)\n\r",px,py); } */ } }