alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
Diff: main.cpp
- Revision:
- 6:405ec2bba6d0
- Parent:
- 5:8e326d07f125
- Child:
- 7:de221f894d3b
--- a/main.cpp Wed Oct 31 16:08:45 2018 +0000 +++ b/main.cpp Thu Nov 01 18:50:26 2018 +0000 @@ -20,15 +20,22 @@ 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(4); +HIDScope scope(6); Ticker scopeTimer; Ticker EMGTicker; @@ -41,16 +48,24 @@ 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); +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.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); +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 @@ -77,7 +92,7 @@ double GainP2 = 3.0; double GainI2 = 0.0; double GainD2 = 0.235; -double TickerPeriod = 1.0/400.0; +double TickerPeriod = 1.0/500.0; // Global variables EMG double EMGdata1; double EMGdata2; @@ -102,8 +117,8 @@ int countstep = 0; //Demo variabelen -double vxMax = 4.0; -double vyMax = 4.0; +double vxMax = 1.0; +double vyMax = 1.0; int DemoStage = 0; //States @@ -120,10 +135,12 @@ void ReadEMG() { + EMGdata1 = a0.read(); // store values in the scope EMGdata2 = a1.read(); EMGdata3 = a2.read(); EMGdata4 = a3.read(); + } // EMG High pass filters @@ -250,17 +267,19 @@ } - scope.set(0, unit_vX); - scope.set(1, unit_vY); - scope.set(2, EMG_filt1); - scope.set(3, EMG_filt2); + 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("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); + pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref); count = 0; } } @@ -286,7 +305,7 @@ void InverseKinematics ()// Kinematics function, takes imput between 1 and -1 { - double V_max = 1.0; //Maximum velocity in either direction + double V_max = 2.5; //Maximum velocity in either direction // CM/s double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta @@ -464,6 +483,41 @@ } } } +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; + } + } + */ +} @@ -534,7 +588,8 @@ //GainI2 = pot2.read() * 10; //GainD2 = pot1.read() ; - DemonstrationPath(); + //DemonstrationPath(); + TestPath(); inverse(); ReadCurrentPosition(); UpdateError(); @@ -579,7 +634,7 @@ counter++; if (counter == 400) // print 1x per seconde waardes. { - pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref); + //pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref); counter = 0; } if (countstep == 4000) @@ -636,18 +691,18 @@ 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 ); - bqc2.add( &HP_emg2 ).add( &Notch_emg2 ); - bqc3.add( &HP_emg3 ).add( &Notch_emg3 ); - bqc4.add( &HP_emg4 ).add( &Notch_emg4 ); + 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++){