2 losse EMG signalen van de biceps en deltoid
Dependencies: HIDScope MODSERIAL mbed-dsp mbed Encoder
Fork of Lampje_EMG_Gr6 by
Diff: main.cpp
- Revision:
- 20:d40b6cba4280
- Parent:
- 19:1bd2fc3bce1e
- Child:
- 21:674fafb6301d
diff -r 1bd2fc3bce1e -r d40b6cba4280 main.cpp --- a/main.cpp Wed Oct 29 08:59:07 2014 +0000 +++ b/main.cpp Wed Oct 29 15:08:30 2014 +0000 @@ -2,18 +2,57 @@ #include "HIDScope.h" #include "arm_math.h" #include "MODSERIAL.h" +#include "encoder.h" -Serial pc(USBTX, USBRX); // tx, rx +#define TSAMP 0.01 +#define K_P (0.1) +#define K_I (0.03 *TSAMP) +#define K_D (0.001 /TSAMP) +#define I_LIMIT 1. + +#define M1_PWM PTC8 +#define M1_DIR PTC9 +#define M2_PWM PTA5 +#define M2_DIR PTA4 + +//Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting + +Serial pc(USBTX, USBRX); + DigitalOut myled1(LED_RED); DigitalOut myled2(LED_GREEN); DigitalOut myled3(LED_BLUE); -PwmOut motorsignal(PTD4); //Define objects AnalogIn emg0(PTB1); //Analog input AnalogIn emg1(PTB2); //Analog input HIDScope scope(2); +//motor 25D +Encoder motor1(PTD3,PTD5,true); //wit, geel +PwmOut pwm_motor1(M2_PWM); +DigitalOut motordir1(M2_DIR); + +//motor2 37D +Encoder motor2(PTD2, PTD0,true); //wit, geel +PwmOut pwm_motor2(M1_PWM); +DigitalOut motordir2(M1_DIR); + +float speed1; +float hoek1; +float speed2; +float hoek2; + +bool flip=false; + +void attime() +{ + flip = !flip; +} + + +// EMG + arm_biquad_casd_df1_inst_f32 lowpass_biceps; arm_biquad_casd_df1_inst_f32 lowpass_deltoid; //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB @@ -211,6 +250,90 @@ myled3 = 1; } +// MOTORFUNCTIES + +void motor2_speed_low () +{ + wait(1); + speed2 = 1; + motordir2=1; + pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. + wait(0.3); //naar 140 graden + pwm_motor2.write(0); //CCW + wait(1); + motordir2=0; + pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. + wait(0.20); //balletje slaan, 160 graden + pwm_motor2.write(0); + wait(1); + motordir2=1; //CW + pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. + wait(1); //terug naar begin positie, 20 graden + pwm_motor2.write(0); +} + +void motor2_speed_mid () +{ + wait(1); + speed2 = 1; + motordir2=1; + pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. + wait(0.3); //naar 140 graden + pwm_motor2.write(0); //CCW + wait(1); + motordir2=0; + pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. + wait(0.20); //balletje slaan, 160 graden + pwm_motor2.write(0); + wait(1); + motordir2=1; //CW + pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. + wait(1); //terug naar begin positie, 20 graden + pwm_motor2.write(0); +} + +void motor2_speed_high () +{ + wait(1); + speed2 = 1; + motordir2=1; + pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn. + wait(0.3); //naar 140 graden + pwm_motor2.write(0); //CCW + wait(1); + motordir2=0; + pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan. + wait(0.20); //balletje slaan, 160 graden + pwm_motor2.write(0); + wait(1); + motordir2=1; //CW + pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan. + wait(1); //terug naar begin positie, 20 graden + pwm_motor2.write(0); +} + +void motor1_links() +{ + speed1 = 0.7; + hoek1 = 0.09; //in seconde + wait(1); + motordir1=0; //aangeven van directie (0 = CCW) + pwm_motor1.write(speed1); //snelheid van de motor + wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait + pwm_motor1.write(0); +} + + +void motor1_rechts() +{ + speed1 = 0.7; + hoek1 = 0.09; //in seconde + wait(1); + motordir1=1; //aangeven van directie (1 = CW) + pwm_motor1.write(speed1); //snelheid van de motor + wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait + pwm_motor1.write(0); +} int main() { @@ -232,6 +355,7 @@ /*Everything is handled by the interrupt routine now!*/ while(1) { + static float count = 0; pc.printf("Span de biceps aan om het instellen te starten.\n"); do { ShineRed(); @@ -241,10 +365,10 @@ BlinkGreen(); while (1) { pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) { + if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) { stopblinkgreen(); - pc.printf("Shinered.\n"); - ShineRed(); + pc.printf("ShineGreen.\n"); + ShineGreen(); wait (4); break; } @@ -252,14 +376,16 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); + motor1_links(); wait(4); break; - } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) { stopblinkgreen(); - pc.printf("ShineGreen.\n"); - ShineGreen(); + pc.printf("ShineRed.\n"); + ShineRed(); + motor1_rechts(); wait (4); break; } @@ -267,10 +393,11 @@ BlinkGreen(); while (1) { pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) { + if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) { stopblinkgreen(); - pc.printf("Shinered.\n"); - ShineRed(); + pc.printf("ShineGreen.\n"); + ShineGreen(); + motor2_speed_mid (); wait (4); break; } @@ -278,14 +405,16 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); + motor2_speed_low (); wait(4); break; - } else if (filtered_average_bi > 0.05 && filtered_average_del > 0.05) + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) { stopblinkgreen(); - pc.printf("ShineGreen.\n"); - ShineGreen(); + pc.printf("ShineRed.\n"); + ShineRed(); + motor2_speed_high (); wait (4); break; }