Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
Diff: main.cpp
- Revision:
- 51:b40967b1fe5c
- Parent:
- 50:c4c9daf7b74c
- Child:
- 52:45200bbde108
diff -r c4c9daf7b74c -r b40967b1fe5c main.cpp --- a/main.cpp Mon Oct 26 15:17:56 2015 +0000 +++ b/main.cpp Mon Oct 26 16:06:26 2015 +0000 @@ -27,7 +27,7 @@ QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor - + // [DEFINE CONSTANTS] // HIDScope scope(3); // Aantal HIDScope kanalen @@ -168,6 +168,8 @@ f++; smp++; pc.printf("Pwm=%f \n\r", pwm_average); + double speed=fabs(pwm_average); + if (pwm_average < 0.1) { leduit(); } if (pwm_average > 0.1) { ledgreen1 = ledon; } @@ -179,10 +181,11 @@ if(smp>512) { - pwm_motor_strike=fabs(pwm_strike); + pwm_motor_strike=speed; + pwm_motor_turn=speed; green(); - wait(5); pwm_motor_strike=0; + wait(3); f=1; smp=0;} } @@ -239,17 +242,29 @@ void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output) { + + //ZONDER NOTCH EMG_left_Bicep = input1; EMG_Right_Bicep = input2; EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); + + EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); - //EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered); - //EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1); - //EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2); - - EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); +// EMG_left_Bicep = input1; EMG_Right_Bicep = input2; +// +// EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); +// +// EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered); +// EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1); +// +// EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2); +// +// +// EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); +// +// EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); } void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)