alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
main.cpp
- Committer:
- laurencebr
- Date:
- 2018-10-31
- Revision:
- 2:58ec7347245e
- Parent:
- 1:23834862b877
- Child:
- 3:53fb8bd0a448
File content as of revision 2:58ec7347245e:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" DigitalOut gpo(D0); DigitalOut led(LED_RED); AnalogIn pot1(A4); //POORTEN VERANDEREN //AnalogIn pot2(); //Beneden is I control op 0 gezet. //POORTEN veranderen AnalogIn pot3(A5); //POORTEN VERANDEREN QEI encoder1(D10, D11, NC, 32); QEI encoder2(D12, D13, NC, 32); FastPWM Motor1PWM(D6); DigitalOut Motor1Direction(D7); FastPWM Motor2PWM(D5); DigitalOut Motor2Direction(D4); //EMG AnalogIn a0(A0); AnalogIn a1(A1); AnalogIn a2(A2); AnalogIn a3(A3); MODSERIAL pc(USBTX, USBRX); //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen HIDScope scope(4); Ticker scopeTimer; Ticker EMGTicker; // BiQuad filters //BiQuad Chains BiQuadChain bqc1; BiQuadChain bqc2; BiQuadChain bqc3; BiQuadChain bqc4; //High pass filters 20 Hz BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189); BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189); BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189); BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189); //Notch filters 50 Hz BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); ///Variables double q1Error; double q2Error; double PrevErrorq1[100]; double PrevErrorq2[100]; int n = 100; // Zelfde waarde als PrevErrorarray double q1Ref = 1.0; //VOOR TESTEN double q2Ref; double xRef; double yRef; double q1Pos; double q2Pos; double PIDerrorq1; double PIDerrorq2; double IntegralErrorq1 = 0.0; double IntegralErrorq2 = 0.0; double DerivativeErrorq1 = 0.0; double DerivativeErrorq2 = 0.0; double GainP1 = 2.0; double GainI1 = 1.2; double GainD1 = 0.0; double GainP2 = 2.0; double GainI2 = 2.0; double GainD2 = 2.0; double TickerPeriod = 1.0/400.0; // Global variables EMG double EMGdata1; double EMGdata2; double EMGdata3; double EMGdata4; int count; double EMG_filt1; double EMG_filt2; double EMG_filt3; double EMG_filt4; double unit_vx; double unit_vY; Ticker Kikker; int counter = 0; int countstep = 0; //EMGDINGEN void ReadEMG() { EMGdata1 = a0.read(); // store values in the scope EMGdata2 = a1.read(); EMGdata3 = a2.read(); EMGdata4 = a3.read(); } // EMG High pass filters double EMG_HP1(double EMGdata) //data 1 { double HP_abs_EMGdata = bqc1.step(EMGdata); return fabs(HP_abs_EMGdata); } double EMG_HP2(double EMGdata) //data 2 { double HP_abs_EMGdata = bqc2.step(EMGdata); return fabs(HP_abs_EMGdata); } double EMG_HP3(double EMGdata) //data 3 { double HP_abs_EMGdata = bqc3.step(EMGdata); return fabs(HP_abs_EMGdata); } double EMG_HP4(double EMGdata) // data 4 { double HP_abs_EMGdata = bqc4.step(EMGdata); return fabs(HP_abs_EMGdata); } // EMG moving filter double EMG_mean (double EMGarray[100]) { double sum = 0.0; for(int j=0; j<100; j++) { sum += EMGarray[j]; } double EMG_filt = sum / 100.0; return EMG_filt; } // Filtered signal to output between -1 and 1 double filt2kin (double EMG_filt1, double EMG_filt2, double max1, double max2) { double EMG_scaled1 = EMG_filt1 / max1; double EMG_scaled2 = EMG_filt2 / max2; double kin_input = EMG_scaled2 - EMG_scaled1; if (kin_input > 1.0) { kin_input = 1.0; } if (kin_input < -1.0) { kin_input = -1.0; } return kin_input; } void EMG () { ReadEMG(); double HP_abs_EMGdata1 = EMG_HP1(EMGdata1); double HP_abs_EMGdata2 = EMG_HP2(EMGdata2); double HP_abs_EMGdata3 = EMG_HP3(EMGdata3); double HP_abs_EMGdata4 = EMG_HP4(EMGdata4); static double EMG_array1[100]; static double EMG_array2[100]; static double EMG_array3[100]; static double EMG_array4[100]; // Fill array 1 for(int i = 100-1; i >= 1; i--) { EMG_array1[i] = EMG_array1[i-1]; } EMG_array1[0] = HP_abs_EMGdata1; // Fill array 2 for(int i = 100-1; i >= 1; i--) { EMG_array2[i] = EMG_array2[i-1]; } EMG_array2[0] = HP_abs_EMGdata2; // Fill array 3 for(int i = 100-1; i >= 1; i--) { EMG_array3[i] = EMG_array3[i-1]; } EMG_array3[0] = HP_abs_EMGdata3; // Fill array 4 for(int i = 100-1; i >= 1; i--) { EMG_array4[i] = EMG_array4[i-1]; } EMG_array4[0] = HP_abs_EMGdata4; EMG_filt1 = EMG_mean(EMG_array1); //global maken EMG_filt2 = EMG_mean(EMG_array2); EMG_filt3 = EMG_mean(EMG_array3); EMG_filt4 = EMG_mean(EMG_array4); unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2); unit_vY = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4); scope.set(0, unit_Vx); scope.set(1, unit_Vy); //scope.set(2, EMG_filt3); //scope.set(3, EMG_filt4); count++; if (count == 100) { pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4); count = 0; } } //PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS //InverseKinematics void inverse(double X1, double Y1, double & thetaM1, double & thetaM2) { double L1 = 40.0; // %define length of arm 1 attached to gear double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0)); double q3 = atan(Y1/X1); double q4 = 2.0*asin(0.5*L3/L1); thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0; thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0; } void InverseKinematics ()// Kinematics function, takes imput between 1 and -1 { double V_max = 1.0; //Maximum velocity in either direction // CM/s double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta double deltaY = TickerPeriod*V_max*unit_vY; static double X1; static double Y1; X1 += deltaX; Y1 += deltaY; double THETA1; double THETA2; inverse(X1, Y1, THETA1, THETA2); q1Ref = THETA1; q2Ref = THETA2; } void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos { q1Pos = encoder1.getPulses()/32/131.25*2*3.1416; //Position motor 1 in rad q2Pos = encoder2.getPulses()/32/131.25*2*3.1416; //Position motor 2 in rad } void UpdateError() //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos { q1Error = q1Ref - q1Pos; q2Error = q2Ref - q2Pos; //Update PrevErrorq1 and PrevErrorq2 for (int i = 0; i <= n-2 ; i++) { PrevErrorq1[i] = PrevErrorq1[i+1]; PrevErrorq2[i] = PrevErrorq2[i+1]; } PrevErrorq1[n-1] = q1Error; PrevErrorq2[n-1] = q2Error; } void PIDControl(){ // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error // PID control motor 1 //P-Control double P_error1 = GainP1 * q1Error; //I-Control IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod; double I_error1 = GainI1 * IntegralErrorq1; //D-Control //First smoothen the error signal double MovAvq1_1 = 0.0; double MovAvq1_2 = 0.0; for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1)); MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1)); } DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; double D_error1 = GainD1 * DerivativeErrorq1; // Derivative error sum over all time? PIDerrorq1 = P_error1 + I_error1 + D_error1; // PID control motor 2 //P-Control double P_error2 = GainP2 * q2Error; //I-Control IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod; double I_error2 = GainI2 * IntegralErrorq2; //D-Control //First smoothen the error signal double MovAvq2_1 = 0.0; double MovAvq2_2 = 0.0; for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1)); MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1)); } DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod; double D_error2 = GainD2 * DerivativeErrorq2; // Derivative error sum over all time? PIDerrorq2 = P_error2 + I_error2 + D_error2; } void MotorControl() { //Motor 1 //Keep signal within bounds if (PIDerrorq1 > 1.0){ PIDerrorq1 = 1.0; } else if (PIDerrorq1 < -1.0){ PIDerrorq1 = -1.0; } //Direction if (PIDerrorq1 <= 0){ Motor1Direction = 0; Motor1PWM.write(-PIDerrorq1); //write Duty cycle } if (PIDerrorq1 >= 0){ Motor1Direction = 1; Motor1PWM.write(PIDerrorq1); //write Duty cycle } //Motor 2 //Keep signal within bounds if (PIDerrorq2 > 1.0){ PIDerrorq2 = 1.0; } else if (PIDerrorq2 < -1.0){ PIDerrorq2 = -1.0; } //Direction if (PIDerrorq2 <= 0){ Motor2Direction = 0; Motor2PWM.write(-PIDerrorq2); //write Duty cycle } if (PIDerrorq2 >= 0){ Motor2Direction = 1; Motor2PWM.write(PIDerrorq2); //write Duty cycle } } void NormalOperatingModus() { EMG() InverseKinematics(); ReadCurrentPosition(); UpdateError(); PIDControl(); MotorControl(); scope.set(0, q1Pos); scope.set(1, q1Ref); GainP1 = pot3.read() * 10; GainI1 = 0; //pot2.read() * 10; GainD1 = pot1.read() ; countstep++; counter++; if (counter == 400) // print 1x per seconde waardes. { pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref); counter = 0; } if (countstep == 4000) { q1Ref = !q1Ref; countstep = 0; } } int main() { pc.baud(115200); //BiQuad chains bqc1.add( &HP_emg1 ).add( &Notch_emg1 ); bqc2.add( &HP_emg2 ).add( &Notch_emg2 ); bqc3.add( &HP_emg3 ).add( &Notch_emg3 ); bqc4.add( &HP_emg4 ).add( &Notch_emg4 ); //Initialize array errors to 0 for (int i = 0; i <= 9; i++){ PrevErrorq2[i] = 0; PrevErrorq2[i] = 0; } int frequency_pwm = 16700; //16.7 kHz PWM Motor1PWM.period(1.0/frequency_pwm); // T = 1/f Motor2PWM.period(1.0/frequency_pwm); // T = 1/f Kikker.attach(NormalOperatingModus, TickerPeriod); scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!! // Attach the HIDScope::send method from the scope object to the timer at 50Hz scopeTimer.attach_us(&scope, &HIDScope::send, 5e3); //EMGTicker.attach_us(EMG_filtering, 5e3); //. while(true); {} }