Motor programma met EMG
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
main.cpp
- Committer:
- Margreeth95
- Date:
- 2015-10-12
- Revision:
- 28:a40884792e0a
- Parent:
- 27:3392f03bfada
- Child:
- 29:c34c2c3d30a9
File content as of revision 28:a40884792e0a:
//--------------------------------------------------------------------------------------------------------------------------// // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7 //--------------------------------------------------------------------------------------------------------------------------// // Libraries //--------------------------------------------------------------------------------------------------------------------------// #include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "biquadFilter.h" //--------------------------------------------------------------------------------------------------------------------------// // Constanten/Inputs/Outputs //--------------------------------------------------------------------------------------------------------------------------// MODSERIAL pc(USBTX, USBRX); // To/From PC QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1 HIDScope scope(5); // Scope, 4 channels DigitalIn Button1(SW3); DigitalIn Button2(SW2); // LEDs DigitalOut LedR(LED_RED); DigitalOut LedG(LED_GREEN); DigitalOut LedB(LED_BLUE); // Motor DigitalOut motor1direction(D7); // Motor 1, Direction & Speed PwmOut motor1speed(D6); DigitalOut motor2direction(D4); // Motor 2, Direction & Speed PwmOut motor2speed(D5); //EMG AnalogIn EMG_left(A0); //Analog input AnalogIn EMG_right(A1); // Tickers Ticker ScopeTime; Ticker myControllerTicker2; Ticker myControllerTicker1; Ticker SampleEMG; Ticker ScopeTimer; Ticker serial; Ticker MovingAverage; // Constants double reference2, reference1; double position2 = 0, position1 = 0; double m2_ref = 0, m1_ref = 0; int count = 0; double Grens2 = 90, Grens1 = 90; double Stapgrootte = 5; double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0; double EMG_L_fh=0; double EMG_left_value; double EMG_left_f1; double EMG_left_f2; double EMG_left_f3; double EMG_left_f4; double EMG_left_f5; double EMG_left_f6; double EMG_left_abs; double EMG_right_value; double EMG_right_f1; double EMG_right_f2; double EMG_right_abs; double Threshold1 = 0.02; double Threshold2 = 0.06; int N = 50; double MAF_left[50]; double EMG_left_MAF; //Sample time (motor-step) const double m2_Ts = 0.01, m1_Ts = 0.01; //Controller gain Motor 2 & 1 const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20; const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20; double m2_err_int = 0, m2_prev_err = 0; double m1_err_int = 0, m1_prev_err = 0; //Derivative filter coeffs Motor 2 & 1 const double BiGain2 = 0.012, BiGain1 = 0.016955; const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2; const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1; // coëfficiënten const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763; const double EMGH1_a1 = -1.56308931068*BiGainEMG_H1, EMGH1_a2 = 0.61765749583*BiGainEMG_H1, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter const double EMGH2_a1 = -1.75651417587*BiGainEMG_H2, EMGH2_a2 = 0.82183182692*BiGainEMG_H2, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396; const double EMGL1_a1 = -1.55576653052*BiGainEMG_L1, EMGL1_a2 = 0.61374320375*BiGainEMG_L1, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter const double EMGL2_a1 = -1.79696141922*BiGainEMG_L2, EMGL2_a2 = 0.85096669383*BiGainEMG_L2, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081; const double EMGN1_a1 = -1.56858163035*BiGainEMG_N1, EMGN1_a2 = 0.96424138362*BiGainEMG_N1, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for high-pass filter const double EMGN2_a1 = -1.61100357722*BiGainEMG_N2, EMGN2_a2 = 0.96592170538*BiGainEMG_N2, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for high-pass filter // Filter variables double m2_f_v1 = 0, m2_f_v2 = 0; double m1_f_v1 = 0, m1_f_v2 = 0; // Creating the filters biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2); biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2); biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2); //--------------------------------------------------------------------------------------------------------------------------// // General Functions //--------------------------------------------------------------------------------------------------------------------------// //HIDScope void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt { scope.set(0, reference2 - position2); scope.set(1, position2); scope.set(2, reference1 - position1); scope.set(3, position1); scope.set(4, EMG_left_MAF); scope.send(); } // Biquad filter double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } // Reusable PID controller double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2) { // Derivative double e_der = (e-e_prev)/Ts; e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); e_prev = e; // Integral e_int = e_int + Ts*e; // PID return Kp * e + Ki*e_int + Kd*e_der; } // Movingaverage Filter void MovingAverageFilter() { EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N; MAF_left[49] = MAF_left[48]; MAF_left[48] = MAF_left[47]; MAF_left[47] = MAF_left[46]; MAF_left[46] = MAF_left[45]; MAF_left[45] = MAF_left[44]; MAF_left[44] = MAF_left[43]; MAF_left[43] = MAF_left[42]; MAF_left[42] = MAF_left[41]; MAF_left[41] = MAF_left[40]; MAF_left[40] = MAF_left[39]; MAF_left[39] = MAF_left[38]; MAF_left[38] = MAF_left[37]; MAF_left[37] = MAF_left[36]; MAF_left[36] = MAF_left[35]; MAF_left[35] = MAF_left[34]; MAF_left[34] = MAF_left[33]; MAF_left[33] = MAF_left[32]; MAF_left[32] = MAF_left[31]; MAF_left[31] = MAF_left[30]; MAF_left[30] = MAF_left[29]; MAF_left[29] = MAF_left[28]; MAF_left[28] = MAF_left[27]; MAF_left[27] = MAF_left[26]; MAF_left[26] = MAF_left[25]; MAF_left[25] = MAF_left[24]; MAF_left[24] = MAF_left[23]; MAF_left[23] = MAF_left[22]; MAF_left[22] = MAF_left[21]; MAF_left[21] = MAF_left[20]; MAF_left[20] = MAF_left[19]; MAF_left[19] = MAF_left[18]; MAF_left[18] = MAF_left[17]; MAF_left[17] = MAF_left[16]; MAF_left[16] = MAF_left[15]; MAF_left[15] = MAF_left[14]; MAF_left[14] = MAF_left[13]; MAF_left[13] = MAF_left[12]; MAF_left[12] = MAF_left[11]; MAF_left[11] = MAF_left[10]; MAF_left[10] = MAF_left[9]; MAF_left[9] = MAF_left[8]; MAF_left[8] = MAF_left[7]; MAF_left[7] = MAF_left[6]; MAF_left[6] = MAF_left[5]; MAF_left[5] = MAF_left[4]; MAF_left[4] = MAF_left[3]; MAF_left[3] = MAF_left[2]; MAF_left[2] = MAF_left[1]; MAF_left[1] = MAF_left[0]; MAF_left[0] = EMG_left_f6; } //--------------------------------------------------------------------------------------------------------------------------// //EMG functions //--------------------------------------------------------------------------------------------------------------------------// // EMG filtering function void EMGfilter() // Both EMG signals are filtered in one function and with the same filters { EMG_left_value = EMG_left.read(); EMG_left_f1 = EMG_highpass1.step(EMG_left_value); EMG_left_f2 = EMG_highpass2.step(EMG_left_f1); EMG_left_abs = fabs(EMG_left_f2); EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs); EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3); EMG_left_f5 = EMG_notch1.step(EMG_left_f4); EMG_left_f6 = EMG_notch1.step(EMG_left_f5); // EMG_right_value = EMG_right.read(); // EMG_right_f1 = EMG_highpass.step(EMG_right_value); // EMG_right_f1 = EMG_lowpass.step(EMG_right_f1); // EMG_right_abs = fabs(EMG_right_f2); } //--------------------------------------------------------------------------------------------------------------------------// // Motor control functions //--------------------------------------------------------------------------------------------------------------------------// // Motor2 control void motor2_Controller() { // Setpoint motor 2 reference2 = m2_ref; // Reference in degrees position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees // Speed control double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2); double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input motor2speed = abs(m2_P2); // Direction control if(m2_P2 > 0) { motor2direction = 0; } else { motor2direction = 1; } } // Motor1 control void motor1_Controller() { // Setpoint Motor 1 reference1 = m1_ref; // Reference in degrees position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees // Speed control double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); motor1speed = abs(m1_P2); // Direction control if(m1_P2 > 0) { motor1direction = 1; } else { motor1direction = 0; } } //--------------------------------------------------------------------------------------------------------------------------// // Main function //--------------------------------------------------------------------------------------------------------------------------// int main() { //--------------------------------------------------------------------------------------------------------------------------// // Initalizing //--------------------------------------------------------------------------------------------------------------------------// //LEDs OFF LedR = LedB = LedG = 1; //PC connection & check pc.baud(115200); pc.printf("Tot aan loop werkt\n"); // Tickers ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1 SampleEMG.attach(&EMGfilter, 0.01f); MovingAverage.attach(&MovingAverageFilter, 0.01f); //--------------------------------------------------------------------------------------------------------------------------// // Control Program //--------------------------------------------------------------------------------------------------------------------------// while(true) { char c = pc.getc(); // 1 Program UP if(c == 'e') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs >= Threshold1)) { count = count + 1; if(count > 2) { count = 2; } } // 1 Program DOWN if(c == 'd') // Hoe gaat dit aangestuurd worden? { count = count - 1; if(count < 0) { count = 0; } } // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED if(count == 0) { LedR = LedB = 1; LedG = 0; if(c == 'r') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) // { m2_ref = m2_ref + Stapgrootte; m1_ref = m1_ref - Stapgrootte; if (m2_ref > Grens2) { m2_ref = Grens2; m1_ref = -1*Grens1; } } if (c == 'f') //&& (EMG_right_abs < Threshold1)) { m2_ref = m2_ref - Stapgrootte; m1_ref = m1_ref + Stapgrootte; if (m2_ref < -1*Grens2) { m2_ref = -1*Grens2; m1_ref = Grens1; } } } // PROGRAM 1: Motor 1 control, Red LED if(count == 1) { LedG = LedB = 1; LedR = 0; if(c == 't') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) // { m1_ref = m1_ref + Stapgrootte; if (m1_ref > Grens1) { m1_ref = Grens1; } } if(c == 'g') //if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) { m1_ref = m1_ref - Stapgrootte; if (m1_ref < -1*Grens1) { m1_ref = -1*Grens1; } } } // PROGRAM 2: Firing mechanism & Reset, Blue LED if(count == 2) { LedR = LedG = 1; LedB = 0; //VUUUUR!! (To Do) wait(1); m2_ref = 0; m1_ref = 0; count = 0; } } }