alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
main.cpp
- Committer:
- laurencebr
- Date:
- 2018-11-01
- Revision:
- 6:405ec2bba6d0
- Parent:
- 5:8e326d07f125
- Child:
- 7:de221f894d3b
File content as of revision 6:405ec2bba6d0:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" #include "BiQuad.h" DigitalIn ButtonCal(D2); //Button 1 DigitalOut gpo(D0); DigitalOut ledred(LED_RED); DigitalOut ledblue(LED_BLUE); DigitalOut ledgreen(LED_GREEN); AnalogIn pot1(A4); //POORTEN VERANDEREN //AnalogIn pot2(A3); //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); /*AnalogIn Ppot(A0); AnalogIn Ipot(A1); AnalogIn Dpot(A2); */ MODSERIAL pc(USBTX, USBRX); //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen HIDScope scope(6); 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.647459981076977,0.700896781188403); BiQuad HP_emg2(1,-2,1,-1.647459981076977,0.700896781188403); BiQuad HP_emg3(1,-2,1,-1.647459981076977,0.700896781188403); BiQuad HP_emg4(1,-2,1,-1.647459981076977,0.700896781188403); //Notch filters 50 Hz BiQuad Notch_emg1(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); BiQuad Notch_emg2(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); BiQuad Notch_emg3(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); BiQuad Notch_emg4(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003); //LP 98 BiQuad LP1(1,2,1,1.911197067426073,0.914975834801434); BiQuad LP2(1,2,1,1.911197067426073,0.914975834801434); BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434); BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434); ///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 = 40.0; double yRef = 40.0; 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 = 3.0; double GainI1 = 0.0; double GainD1 = 0.235; double GainP2 = 3.0; double GainI2 = 0.0; double GainD2 = 0.235; double TickerPeriod = 1.0/500.0; // Global variables EMG double EMGdata1; double EMGdata2; double EMGdata3; double EMGdata4; int count = 0; double EMG_filt1; double EMG_filt2; double EMG_filt3; double EMG_filt4; double EMG_max1 = 10000.0; double EMG_max2 = 10000.0; double EMG_max3 = 10000.0; double EMG_max4 = 10000.0; double unit_vX; double unit_vY; Ticker Kikker; int counter = 0; int countstep = 0; //Demo variabelen double vxMax = 1.0; double vyMax = 1.0; int DemoStage = 0; //States enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState}; states State = WaitModusState; //Calibration Time int countcalibration = 0; double CalibrationTime = 20.0; //Tijd om te calibreren seconden //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); if (fabs(unit_vX)<0.1) { unit_vX = 0.0; } if (fabs(unit_vY)<0.1) { unit_vY = 0.0; } scope.set(0, EMG_filt1); scope.set(1, EMG_filt2); scope.set(2, unit_vX); scope.set(3, EMG_filt3); scope.set(4, EMG_filt4); scope.set(5, unit_vY); count++; if (count == 100) { pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref); count = 0; } } //PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS //InverseKinematics void inverse() { double L1 = 40.0; // %define length of arm 1 attached to gear double L3 = sqrt(pow(xRef,2.0)+pow(yRef,2.0)); double q3 = atan(yRef/xRef); double q4 = 2.0*asin(0.5*L3/L1); q1Ref = (0.5*3.1416-0.5*q4+q3)*9.0; q2Ref = (3.1416-q1Ref/9.0-q4)*4.0; } void InverseKinematics ()// Kinematics function, takes imput between 1 and -1 { double V_max = 2.5; //Maximum velocity in either direction // CM/s double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta double deltaY = TickerPeriod*V_max*unit_vY; xRef += deltaX; yRef += deltaY; inverse(); } void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos { q1Pos = -encoder1.getPulses()/32/131.25*2*3.1416 + 3.1416/2.0*9.0; //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){ //tijdelijk 0.6, hoort 1.0 te zijn PIDerrorq1 = 1.0; } else if (PIDerrorq1 < -1.0){ PIDerrorq1 = -1.0; } //Direction if (PIDerrorq1 <= 0){ Motor1Direction = 1; Motor1PWM.write(-PIDerrorq1); //write Duty cycle } if (PIDerrorq1 >= 0){ Motor1Direction = 0; 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 = 1; Motor2PWM.write(-PIDerrorq2); //write Duty cycle } if (PIDerrorq2 >= 0){ Motor2Direction = 0; Motor2PWM.write(PIDerrorq2); //write Duty cycle } } void DemonstrationPath() { // Also think about how to move from whatever position to (40,5) if (DemoStage == 0) //From (40,40) to (40,-5) { if (yRef >0) { yRef = yRef - vyMax * TickerPeriod; } else { DemoStage = 1; } } else if (DemoStage == 1) //From (40,-5) to (65,-5) { if (xRef > 30) { xRef = xRef - vxMax * TickerPeriod; } else { DemoStage = 2; } } else if (DemoStage == 2) { if (yRef < 10) //From (65,-5) to (65, 10) { yRef = yRef + TickerPeriod; } else { DemoStage = 3; } } else if (DemoStage == 3) //From (65,10) to (40,10) { if (xRef < 40) { xRef = xRef + vxMax * TickerPeriod; } else { DemoStage = 0; // Repeat moving in the square pattern } } } void TestPath() { /* GainP1 = Ppot.read()*60.0; GainI1 = Ipot.read()*20.0; GainD1 = Dpot.read(); GainP2 = Ppot.read()*30.0; GainI2 = Ipot.read()*20.0; GainD2 = Dpot.read(); pc.printf("GainP = %1f, GainI = %1f, GainD = %1f", GainP1, GainI1, GainD1); // Also think about how to move from whatever position to (40,5) if (DemoStage == 0) //From (40,40) to (40,-5) { if (yRef >0) { yRef = yRef - vyMax * TickerPeriod; } else { DemoStage = 1; } } else if (DemoStage == 1) { if (yRef < 30) //From (65,-5) to (65, 10) { yRef = yRef + vyMax * TickerPeriod; } else { DemoStage = 0; } } */ } void CalibrationButton() { ledred = 1; ledgreen = 1; ledblue = 0; EMG_max1 = 0.0001; EMG_max2 = 0.0001; EMG_max3 = 0.0001; EMG_max4 = 0.0001; State = EMGCalibrationState; } void EMGCalibration() { if (0.95*EMG_filt1>EMG_max1) { EMG_max1 = 0.95*EMG_filt1; } if (0.95*EMG_filt2>EMG_max2) { EMG_max2 = 0.95*EMG_filt2; } if (0.95*EMG_filt3>EMG_max3) { EMG_max3 = 0.95*EMG_filt3; } if (0.95*EMG_filt4>EMG_max4) { EMG_max4 = 0.95*EMG_filt4; } } void CalibrationModus() { EMG(); EMGCalibration(); countcalibration++; pc.printf("countcal = %i", countcalibration); if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod)) { State = NormalOperatingModusState; countcalibration = 0; } } void DemoModus() // The ticker should call this function (in the switch statement) { //GainP1 = pot3.read() * 30; //GainI1 = pot2.read() * 10; //GainD1 = pot1.read() ; //GainP2 = pot3.read() * 10; //GainI2 = pot2.read() * 10; //GainD2 = pot1.read() ; //DemonstrationPath(); TestPath(); inverse(); ReadCurrentPosition(); UpdateError(); PIDControl(); MotorControl(); //scope.set(0, q1Pos); // scope.set(1, q1Ref); //scope.set(2, q2Pos); //scope.set(3, q2Ref); count++; if (count ==400) { pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); count = 0; } } void NormalOperatingModus() { ledred = 1; ledgreen = 0; ledblue = 1; EMG(); InverseKinematics(); ReadCurrentPosition(); UpdateError(); PIDControl(); MotorControl(); //scope.set(0, q1Pos); //scope.set(1, q1Ref); //GainP1 = pot3.read() * 10; //GainI1 = pot2.read() * 10; //GainD1 = pot1.read() ; countstep++; counter++; if (counter == 400) // print 1x per seconde waardes. { //pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref); counter = 0; } if (countstep == 4000) { q1Ref = !q1Ref; countstep = 0; } } void StateMachine() { if (ButtonCal.read() == 0) { CalibrationButton(); pc.printf("print iets"); } switch(State) { case WaitModusState: //aanmaken EMG(); pc.printf("Wait \n\r"); break; case EMGCalibrationState: CalibrationModus(); //pc.printf("EMG CAL \n\r"); break; case NormalOperatingModusState: NormalOperatingModus(); //pc.printf("NOMS \n\r"); break; case DemoModusState: DemoModus(); pc.printf("Demo \n\r"); break; default : } } int main() { pc.baud(115200); /* GainP1 = pot3.read() * 10; GainI1 = pot2.read() * 10; GainD1 = pot1.read(); */ pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1); wait(7.0); pc.printf("nog 3 seconden \n\r"); wait(3.0); //BiQuad chains bqc1.add( &HP_emg1 ).add( &Notch_emg1 );//.add( &LP1); bqc2.add( &HP_emg2 ).add( &Notch_emg2 );//.add( &LP2); bqc3.add( &HP_emg3 ).add( &Notch_emg3 );//.add( &LP3); bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4); //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 //Emg Calibratie button //ButtonCal.fall(&CalibrationButton); Kikker.attach(StateMachine, 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); {} }