alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
Diff: main.cpp
- Revision:
- 1:23834862b877
- Parent:
- 0:2aa29a0824df
- Child:
- 2:58ec7347245e
--- a/main.cpp Tue Oct 30 14:17:16 2018 +0000 +++ b/main.cpp Tue Oct 30 15:57:25 2018 +0000 @@ -6,22 +6,47 @@ DigitalOut gpo(D0); DigitalOut led(LED_RED); -AnalogIn pot1(A0); -AnalogIn pot2(A1); -AnalogIn pot3(A2); +AnalogIn pot1(A0); //POORTEN VERANDEREN +AnalogIn pot2(A1); //POORTEN veranderen +AnalogIn pot3(A2); //POORTEN VERANDEREN QEI encoder1(D10, D11, NC, 32); QEI encoder2(D12, D13, NC, 32); FastPWM Motor1PWM(D6); DigitalOut Motor1Direction(D7); FastPWM Motor2PWM(D5); //!!!!! Juiste poorten aangeven DigitalOut Motor2Direction(D4); //!!!!! Juiste poort aangeven +//EMG +AnalogIn a0(A0); +AnalogIn a1(A1); +AnalogIn a2(A2); +AnalogIn a3(A3); MODSERIAL pc(USBTX, USBRX); -//HIDSCOPE +//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 @@ -32,8 +57,6 @@ int n = 100; // Zelfde waarde als PrevErrorarray double q1Ref = 1.0; //VOOR TESTEN double q2Ref; -//Double xPos; -//Double yPos; double xRef; double yRef; double q1Pos; @@ -51,25 +74,202 @@ 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 count = 0; +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; + } +} + + -void UpdateRefPosition() +//PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS + + +//InverseKinematics + +void inverse(double X1, double Y1, double & thetaM1, double & thetaM2) { - // Update, (based on EMG or pots) the reference position xRef, yRef - //q1Ref = pot1.read()*2*3.1416; - //q2Ref = pot2.read()*2*3.1416; -} + 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() +void InverseKinematics ()// Kinematics function, takes imput between 1 and -1 { - // Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref -// (So, update the values of q1Ref and q2Ref) + + 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; } @@ -188,7 +388,7 @@ void NormalOperatingModus() { - UpdateRefPosition(); + EMG() InverseKinematics(); ReadCurrentPosition(); UpdateError(); @@ -203,11 +403,11 @@ GainD1 = pot1.read() ; countstep++; - count++; - if (count == 400) // print 1x per seconde waardes. + 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); - count = 0; + counter = 0; } if (countstep == 4000) { @@ -222,6 +422,13 @@ 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; @@ -235,6 +442,12 @@ 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); {} }