Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 5:b9d5d7311dac
- Parent:
- 4:b4530fb376dd
- Child:
- 6:1597888c9a56
diff -r b4530fb376dd -r b9d5d7311dac main.cpp --- a/main.cpp Wed Oct 21 11:50:15 2015 +0000 +++ b/main.cpp Thu Oct 22 13:11:55 2015 +0000 @@ -9,11 +9,17 @@ PwmOut led(D9); DigitalIn button_1(PTC6); //counterclockwise DigitalIn button_2(PTA4); //clockwise +AnalogIn PotMeter1(A5); +AnalogIn EMG(A0); +//AnalogIn EMG_bicepsright (A1); +//AnalogIn EMG_legleft (A2); +//AnalogIn EMG_legright (A3); +Ticker controller; +Ticker ticker_regelaar; +Ticker EMG_Control; +//Timer Timer_Calibration; Encoder motor1(D12,D13); -HIDScope scope(1); -AnalogIn PotMeter1(A1); -Ticker controller; -Ticker ticker_regelaar; +HIDScope scope(3); MODSERIAL pc(USBTX, USBRX); volatile bool regelaar_ticker_flag; @@ -26,7 +32,7 @@ #define SAMPLETIME_REGELAAR 0.005 //define states -enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; +enum state {HOME, CALIBRATIE, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; uint8_t state = HOME; // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering @@ -61,6 +67,78 @@ return motor1_Kp*error + motor1_Ki*err_int; } // De totale fout die wordt hersteld met behulp van PI control. +//bool Cali = false; +//double TimeCali = 5; + +// Filter1 = High pass filter tot 20 Hz +double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; +const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; +const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; +// Filter2 = Low pass filter na 60 Hz +double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; +const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; +const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; +// Filter3 = Notch filter at 50 Hz +double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; +const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; +const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1; +const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1; + +// Filter4 = Lowpass filter at 5 Hz +double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0; +const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0; +const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1; + +double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7, u8, u9; +double final_filter1; + +// Standaard formule voor het biquad filter +double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) + +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2=v1; + v1=v; + return y; +} + +void Filters() +{ + u1 = EMG.read(); + //Highpass + y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); + u2 = y1; + y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885); + //Lowpass + u3 = y2; + y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103); + u4 = y3; + y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617); + // Notch + u5 = y4; + y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206); + u6 = y5; + y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2); + u7 = y6; + y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227); + y8 = fabs (y7); + //smooth + u8 = y8; + y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386); + u9 = y9; + final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109); + + scope.set (0, y8); + // scope.set (1, y2); + // scope.set (2, y4); + // scope.set (3, y7); + scope.set (1, final_filter1); + //scope.set (2, final_filter1); + scope.send (); + } + + void motor1_controlPI() { double referencePI1 = PotMeter1.read(); @@ -107,22 +185,27 @@ void move_motor1() { - if (button_1 == pressed) { - move_motor1_cw (); - } else if (button_2 == pressed) { - move_motor1_ccw (); - } else { - motor1_speed = 0; - } + if (final_filter1 > 0.03){ + pc.printf("Moving clockwise \n\r"); + move_motor1_cw (); + } + else if (button_2 == pressed){ + pc.printf("Moving counterclockwise \n\r"); + move_motor1_ccw (); + } + else { + pc.printf("Not moving \n\r"); + motor1_speed = 0; + } } -void read_encoder1 () // aflezen van encoder via hidscope?? -{ - scope.set(0,motor1.getPosition()); - led.write(motor1.getPosition()/100.0); - scope.send(); - wait(0.2f); -} +//void read_encoder1 () // aflezen van encoder via hidscope?? +//{ +// scope.set(0,motor1.getPosition()); + //led.write(motor1.getPosition()/100.0); +// scope.send(); +// wait(0.2f); +//} void print_position(){ pc.printf("move motor \n\r"); @@ -132,20 +215,41 @@ { while (true) { pc.baud(9600); //pc baud rate van de computer + EMG_Control.attach_us(Filters,1e3); switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases case HOME: //positie op 0 zetten voor arm 1 { pc.printf("home\n\r"); - read_encoder1(); + //read_encoder1(); gethome(); pc.printf("Home-position is %f\n\r", H); - state = MOVE_MOTOR; + state = CALIBRATIE; wait(5); break; } + //case CALIBRATIE: + //{ + //pc.printf("Aanspannen in 10 \n\r"); + //wait(10); + //EMG_Control.attach_us(MyController,1e3); + //Timer_Calibration.start(); + //if (Timer_Calibration < TimeCali) { + // Cali = true; + // pc.printf("Aanspannen \n\r"); + //} + //else { + // Cali = false; + //} + //pc.printf("Stoppen \n\r"); + //Timer_Calibration.stop(); + //Timer_Calibration.reset(); + //state = MOVE_MOTOR; + // break; + //} + case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons { pc.printf("move_motor\n\r"); @@ -166,7 +270,8 @@ wait (1); ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); - + //EMG_Control.attach_us(Filters,1e3); + while(state == BACKTOHOMEPOSITION){ movetohome(); while(regelaar_ticker_flag != true);