Facking kut filter werkt EINDELIJK !311!!111!!1!!!!!!!!!!!!!!!!!!!
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of Turning_Motor_V6 by
main.cpp
- Committer:
- Brighton_the_robot
- Date:
- 2018-10-31
- Revision:
- 8:b6b09226a421
- Parent:
- 7:c5c648898881
- Child:
- 9:355babe55589
File content as of revision 8:b6b09226a421:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "BiQuad.h" MODSERIAL pc(USBTX, USBRX); DigitalOut DirectionPin1(D4); DigitalOut DirectionPin2(D7); PwmOut PwmPin1(D5); PwmOut PwmPin2(D6); DigitalIn Knop1(D2); DigitalIn Knop2(D3); AnalogIn pot1 (A5); AnalogIn pot2 (A4); AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); //DigitalOut LED(LED_RED); Ticker StateTicker; Ticker printTicker; HIDScope scope(4); BiQuadChain bqc1; BiQuadChain bqc2; BiQuadChain bqc3; BiQuadChain bqc4; BiQuadChain bqc5; BiQuadChain bqc6; BiQuadChain bqc7; BiQuadChain bqc8; BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); BiQuad BqLP1( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); BiQuad BqLP2( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); BiQuad BqLP3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); BiQuad BqLP4( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); volatile float Bicep_Right = 0.0; volatile float Bicep_Left = 0.0; volatile float Tricep_Right = 0.0; volatile float Tricep_Left = 0.0; volatile const float maxVelocity = 8.4; // in rad/s volatile const double pi = 3.14159265358979; volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand volatile float referenceVelocity2 = 0.5; enum states{Calibration, Homing, Function}; volatile states Active_State = Calibration; volatile int counts1 ; volatile int counts2 ; volatile float rad_m1; volatile float rad_m2; volatile float q_1; volatile float q_2; volatile float r_1; volatile float r_2; volatile const float r_3 = 2.0; // volatile float FilterHP_Bi_R; volatile float Filterabs_Bi_R; volatile float Filtered_Bi_R; volatile float FilterHP_Bi_L; volatile float Filterabs_Bi_L; volatile float Filtered_Bi_L; volatile float FilterHP_Tri_R; volatile float Filterabs_Tri_R; volatile float Filtered_Tri_R; volatile float FilterHP_Tri_L; volatile float Filterabs_Tri_L; volatile float Filtered_Tri_L; void filter() { FilterHP_Bi_R = bqc1.step( emg0.read() ); Filterabs_Bi_R = fabs(FilterHP_Bi_R); Filtered_Bi_R = bqc2.step( Filterabs_Bi_R ); FilterHP_Bi_L = bqc3.step( emg1.read() ); Filterabs_Bi_L = fabs(FilterHP_Bi_L); Filtered_Bi_L = bqc4.step( Filterabs_Bi_L ); FilterHP_Tri_R = bqc5.step( emg2.read() ); Filterabs_Tri_R = fabs(FilterHP_Tri_R); Filtered_Tri_R = bqc6.step( Filterabs_Tri_R ); FilterHP_Tri_L = bqc7.step( emg3.read() ); Filterabs_Tri_L = fabs(FilterHP_Tri_L); Filtered_Tri_L = bqc8.step( Filterabs_Tri_L ); } void Encoding() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); // Hier gaat iets fout waardoor het 0 wordt!!! rad_m1 = ((2.0*pi)/32.0)* (float)counts1; rad_m2 = ((2.0*pi)/32.0)* (float)counts2; // pc.printf("%f & %f ....\n",rad_m1, rad_m2); } void EMG_Read() { Bicep_Right = emg0.read(); Bicep_Left = emg1.read(); Tricep_Right = emg2.read(); Tricep_Left = emg3.read(); } void sample() { scope.set(0, Filtered_Bi_R ); scope.set(1, Filtered_Bi_L ); scope.set(2, Filtered_Tri_R ); scope.set(3, Filtered_Tri_L ); scope.send(); } void velocity1() { if (pot1.read()>0.5f) { // Clockwise rotation referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; } else if (pot1.read() == 0.5f) { referenceVelocity1 = pot1.read() * 0.0f; } else if (pot1.read() < 0.5f) { // Counterclockwise rotation referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; } } void velocity2() { if (pot2.read()>0.5f) { // Clockwise rotation referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; } else if (pot2.read() == 0.5f) { referenceVelocity2 = pot2.read() * 0.0f; } else if (pot2.read() < 0.5f) { // Counterclockwise rotation referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; } } void motor1() { float u = referenceVelocity1; DirectionPin1 = u < 0.0f; PwmPin1 = fabs(u); } void motor2() { float u = referenceVelocity2; DirectionPin2 = u > 0.0f; PwmPin2 = fabs(u); } void Printing() { float v1 = fabs(referenceVelocity1) * maxVelocity; float v2 = fabs(referenceVelocity2) * maxVelocity; //eventueel nog counts -> rad/s //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); pc.printf("%i %i \n",counts1,counts2); } void inverse_kinematics() { float JacPs [2][2]; JacPs[0][0] = 2.0; JacPs[0][1] = 3.0; JacPs[1][0] = 4.0; JacPs[1][1] = 5.0; pc.printf("%f ", JacPs[0][0]); } void StateMachine() { switch (Active_State) { case Calibration: //calibration actions //pc.printf("Calibration State"); if (Knop1==false) { pc.printf("Entering Homing state \n"); Active_State = Homing; } filter(); sample(); //EMG_Read(); Encoding(); break; case Homing: //Homing actions //pc.printf("Homing State"); if (Knop2==false) { pc.printf("Entering Funtioning State \n"); Active_State = Function; } filter(); sample(); //EMG_Read(); Encoding(); break; case Function: //pc.printf("Funtioning State"); if (Knop2==false) { pc.printf("Re-entering Homing State \n"); Active_State = Homing; } else if (Knop1==false) { pc.printf("Re-entering Calibration State \n"); Active_State = Calibration; } filter(); sample(); //EMG_Read(); Encoding(); velocity1(); velocity2(); motor1(); motor2(); break; default: pc.printf("UNKNOWN COMMAND"); } } int main() { pc.baud(115200); PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 ); bqc2.add(&BqLP1); bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 ); bqc4.add(&BqLP2); bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 ); bqc6.add(&BqLP3); bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 ); bqc8.add(&BqLP4); StateTicker.attach(StateMachine, 0.002); printTicker.attach(&Printing, 2.0); while(true) { } }