adsf
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of frdm_gpio1 by
Revision 4:afa85283eb18, committed 2017-11-01
- Comitter:
- DiondeGreef
- Date:
- Wed Nov 01 11:50:05 2017 +0000
- Parent:
- 3:2ffbee47fb38
- Commit message:
- afdd
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 31 21:50:46 2017 +0000 +++ b/main.cpp Wed Nov 01 11:50:05 2017 +0000 @@ -24,8 +24,10 @@ DigitalOut motorDirection(D4); PwmOut motorSpeed(D5); +DigitalOut motorDirection1(D4); //nog goede channel toevoegen +PwmOut motorSpeed1(D5); //nog goede channel toevoegen -MODSERIAL pc(USBTX, USBRX); +MODSERIAL pc(USBTX, USBRX); Ticker sample_timer; Ticker cal_timer; @@ -34,16 +36,19 @@ DigitalOut led2(LED_GREEN); const int size = 100; vector<double> S(size,0); +vector<double> S1(size,0); double meanSum = 0; double maxsignal = 0; +double emgX = 0; +double emgY = 0; double L0 = 0.123; double L1 = 0.37; double L2 = 0.41; -double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen -double q2 = encoder2.getPosition()/131; //Calibreren mist nog -double Periode = 1/1000; //1000 is het aantal Hz +double q1 = encoder1.getPosition()/131.25; //Calibreren nog toevoegen +double q2 = encoder2.getPosition()/131.25; //Calibreren mist nog +double Periode = 1/500; //1000 is het aantal Hz //Filter toevoegen, bestaande uit notch, high- en lowpass filter BiQuadChain Notch; @@ -85,6 +90,16 @@ //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...) //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01); + +//Making matrices globaly +Matrix JAPPAPP(2,2); +Matrix qdot(2,1); +Matrix Vdes(2,1); +Matrix qsetpoint(2,1); + + + + double findRMS(vector<double> array) { @@ -121,13 +136,17 @@ } */ S.erase(S.begin()); + S1.erase(S1.begin()); double b = bqc.step(emg0.read()-0.4542); + double c = bqc.step(emg1.read()-0.4542); S.push_back(b); + S1.push_back(c); //S[0] = bqc.step(emg0.read()-0.4542))); //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542))); - double a = findRMS(S); - scope.set(1, a); - scope.set(2, S[0]); + emgX = findRMS(S); + emgY = findRMS(S1); + scope.set(1, emgX); + //scope.set(2, S[0]); //meanSum = 0; */ /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) * Ensure that enough channels are available (HIDScope scope( 2 )) @@ -136,7 +155,7 @@ /* To indicate that the function is working, the LED is toggled */ led = !led; - motorSpeed.write(0.38080*(a/maxsignal)); + motorSpeed.write(0.38080*(emgX/maxsignal)); motorDirection.write(1); } @@ -156,7 +175,6 @@ double signalfinal = findRMS(S); if (signalfinal >= maxsignal){ maxsignal = signalfinal; //keep changing the maximal signal - pc.baud(9600); pc.printf("%d \n",maxsignal); } } @@ -164,8 +182,27 @@ } } + +void qSetpointSet(){ + // Fill Matrix with data. + JAPPAPP(1,1) = -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); + JAPPAPP(1,2) = -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); + JAPPAPP(2,1) = (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); + JAPPAPP(2,2) = (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); + + // Fill Matrix with data. + Vdes(1,1) = emgX; //Goede code nog toevoegen, Vx + Vdes(2,1) = emgY; //goede code nog toevoegen, Vy + + qdot = JAPPAPP*Vdes; + + qsetpoint(1,1) = q1 + qdot(1,1)*Periode; + qsetpoint(2,1) = q2 + qdot(2,1)*Periode; +} + int main() { +pc.baud(115200); //Constructing the notch filter chain Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ); Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 ); @@ -174,41 +211,19 @@ /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz */ - sample_timer.attach(&sample, 0.002); + sample_timer.attach(&sample, Periode); cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated led2 = 1; /*empty loop, sample() is executed periodically*/ - pc.baud(9600); + DigitalOut myled(LED1); -//--- - Matrix JAPPAPP(2,2); - Matrix qdot(2,1); - Matrix Vdes(2,1); - Matrix qsetpoint(2,1); - - // Fill Matrix with data. - JAPPAPP << -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) - << (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)) << (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); - - - // Fill Matrix with data. - Vdes << emg0.read //Goede code nog toevoegen, Vx - << emg1.read; //goede code nog toevoegen, Vy - - - qdot = JAPPAPP*Vdes; - pc.printf("\r\nJAPPAPPP: "); //Alleen visualisatie - qdot.print(); //Alleen visualisatie - - qsetpoint = q1 + qdot*Periode; - - +//--- while(true) { - - + qdot.print(); //Alleen visualisatie + wait(0.5); } } \ No newline at end of file