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:
- 28:c33a0658605e
- Parent:
- 27:691779624530
- Child:
- 29:d0dab8921e9d
diff -r 691779624530 -r c33a0658605e main.cpp --- a/main.cpp Sat Nov 01 13:43:21 2014 +0000 +++ b/main.cpp Sat Nov 01 16:19:53 2014 +0000 @@ -21,13 +21,15 @@ //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting +// Define objects Serial pc(USBTX, USBRX); +// LED DigitalOut myledred(PTB3); DigitalOut myledgreen(PTB1); DigitalOut myledblue(PTB2); -//Define objects +//EMG AnalogIn emg0(PTB0); //Analog input AnalogIn emg1(PTC2); //Analog input HIDScope scope(4); @@ -42,6 +44,7 @@ PwmOut pwm_motor2(M1_PWM); DigitalOut motordir2(M1_DIR); +// Motor variabelen float pwm1_percentage = 0; float pwm2_percentage = 0; int cur_pos_motor1; @@ -60,8 +63,9 @@ int wait_iterator2 = 0; -// EMG +// EMG Filters (settings en variabelen) +// Filters arm_biquad_casd_df1_inst_f32 lowpass_biceps; arm_biquad_casd_df1_inst_f32 lowpass_deltoid; //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB @@ -110,21 +114,7 @@ } } -/** Looper function -* functions used for Ticker and Timeout should be of type void <name>(void) -* i.e. no input arguments, no output arguments. -* if you want to change a variable that you use in other places (for example in main) -* you will have to make that variable global in order to be able to reach it both from -* the function called at interrupt time, and in the main function. -* To make a variable global, define it under the includes. -* variables that are changed in the interrupt routine (written to) should be made -* 'volatile' to let the compiler know that those values may change outside the current context. -* i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value" -* in the example below, the variable is not re-used in the main function, and is thus declared -* local in the looper function only. -**/ - - +// EMG looper void looper() { /*variable to store value in*/ @@ -164,9 +154,6 @@ // LED AANSTURING -Ticker ledticker; - - void BlinkRed(int n) { for (int i=0; i<n; i++) { @@ -181,6 +168,9 @@ } } +// Ticker voor groen knipperen, zodat tijdens dit knipperen presets gekozen kunnen worden +Ticker ledticker; + void greenblink() { if(myledgreen.read()) @@ -209,35 +199,7 @@ ledticker.detach(); } - -void BlinkGreen1 () -{ - - myledred = 0; - myledgreen = 0; - myledblue = 0; - wait(0.1); - myledred = 0; - myledgreen = 1; - myledblue = 0; - wait(0.1); -} - - -void BlinkBlue(int n) -{ - for (int i=0; i<n; i++) { - myledred = 0; - myledgreen = 0; - myledblue = 0; - wait(0.1); - myledred = 0; - myledgreen = 0; - myledblue = 1; - wait(0.1); - } -} - +// Groen schijnen void ShineGreen () { myledred = 0; @@ -245,6 +207,7 @@ myledblue = 0; } +// Blauw schijnen void ShineBlue () { myledred = 0; @@ -252,6 +215,7 @@ myledblue = 1; } +// Rood schijnen void ShineRed () { myledred = 1; @@ -261,6 +225,9 @@ // MOTORFUNCTIES +// Motor1 = batje +// Motor2 = arm + void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op // maar de locatie van de variabele. { @@ -268,6 +235,7 @@ // *in = het getal dat staat op locatie van in --> waarde van new_pwm } +// PI-regelaar motor1: batje float pid1(float setpoint1, float measurement1) { float error1; @@ -280,6 +248,7 @@ return out_p1 + out_i1; } +// PI-regelaar motor2: arm float pid2(float setpoint2, float measurement2) { float error2; @@ -291,16 +260,21 @@ clamp(&out_i2,-I_LIMIT,I_LIMIT); return out_p2 + out_i2; } + +// Variabelen float prev_setpoint1 = 0; float setpoint1 = 0; float prev_setpoint2 = 0; float setpoint2 = 0; +// Functies motoren + +// Motor1 links draaien void batje_links () { speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht) setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden + if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden, nu 11.3. 2.3 is de verhouding van de tanden setpoint1 = (11.3*2.3*2.0*PI/360); } if(setpoint1 < -(11.3*2.3*2.0*PI/360)) { @@ -309,6 +283,7 @@ prev_setpoint1 = setpoint1; } +// Motor1 rechts draaien void batje_rechts () { speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) @@ -326,7 +301,7 @@ } } - +//Motor1 na links draaien weer terug laten draaien naar beginstand void batje_begin_links () { speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) @@ -340,6 +315,7 @@ prev_setpoint1 = setpoint1; } +//Motor1 na links draaien weer terug laten draaien naar beginstand void batje_begin_rechts () { speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht) @@ -353,6 +329,7 @@ prev_setpoint1 = setpoint1; } +// Motor2 balletje op zn hoogst slaan void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN { speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht) @@ -369,6 +346,7 @@ } } +// Motor2 balletje in het midden slaan void arm_mid () { speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht) @@ -385,6 +363,7 @@ } } +// Motor2 balletje op het laagst slaan void arm_laag () { speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht) @@ -401,6 +380,7 @@ } } +// Motor2 arm terug zetten in beginstand void arm_begin () { speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) @@ -414,6 +394,7 @@ prev_setpoint2 = setpoint2; } +// MOTOR aansturing void looper_motor() { //MOTOR1 @@ -462,7 +443,7 @@ wait_iterator1++; if(wait_iterator1 > 400) { staat1 = 2; - + batje_begin_rechts(); } } @@ -475,7 +456,7 @@ wait_iterator1++; if(wait_iterator1 > 400) { staat1 = 2; - + batje_begin_links (); } } @@ -489,7 +470,7 @@ wait_iterator2++; if(wait_iterator2 > 400) { staat2 = 2; - + arm_begin(); } } @@ -502,7 +483,7 @@ wait_iterator2++; if(wait_iterator2 > 400) { staat2 = 2; - + arm_begin(); } } @@ -515,7 +496,7 @@ wait_iterator2++; if(wait_iterator2 > 400) { staat2 = 2; - + arm_begin(); } } @@ -524,6 +505,7 @@ } +// Hoofdprogramma, hierin staat de aansturing vd LED int main() { @@ -532,52 +514,47 @@ pwm_motor2.period_us(100); motor2.setPosition(0); pc.baud(115200); - + // Ticker EMG signaal meten Ticker log_timer; //set up filters. Use external array for constants arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states); arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states); arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states); arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states); - /**Here you attach the 'void looper(void)' function to the Ticker object - * The looper() function will be called every 0.01 seconds. - * Please mind that the parentheses after looper are omitted when using attach. - */ + // Uitvoeren van ticker EMG, sample frequentie 500Hz log_timer.attach(looper, 0.002); + // Aanroepen van motoraansturing in motor ticker Ticker looptimer; looptimer.attach(looper_motor,TSAMP); - while(1) { //Loop - /*Empty!*/ - /*Everything is handled by the interrupt routine now!*/ + + while(1) { while(1) { pc.printf("Span de biceps aan om het instellen te starten.\n"); do { ShineRed(); } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting - if (filtered_average_bi > 0.05) { - BlinkRed(10); - BlinkGreen(); - while (1) { + if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen + BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking + BlinkGreen(); // groen knipperen, meten van spieraanspanning + while (1) { // eerste loop, keuze voor de positie van het batje pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { + if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden stopblinkgreen(); pc.printf("ShineGreen.\n"); ShineGreen(); wait (4); break; } - if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { + if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); batje_hoek = 2; wait(4); break; - } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) - - { + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed(); @@ -587,9 +564,9 @@ } } BlinkGreen(); - while (1) { + while (1) { // loop voor het instellen van de kracht pc.printf("In de loop.\n"); - if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { + if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan stopblinkgreen(); pc.printf("ShineGreen.\n"); ShineGreen(); @@ -597,16 +574,14 @@ wait (4); break; } - if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { + if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); arm_hoogte = 1; wait(4); break; - } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) - - { + } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed();