Janine Klein Rot
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 14:e1816efa712d
- Parent:
- 13:54ee98850a15
- Child:
- 15:3ebd0e666a9c
--- a/main.cpp Fri Oct 24 11:06:15 2014 +0000
+++ b/main.cpp Mon Oct 27 14:43:50 2014 +0000
@@ -62,9 +62,10 @@
Ticker ticker_regelaar; //Ticker voor regelaar motor
Ticker ticker_EMG; //Ticker voor EMG meten
+Timer biceps_kalibratie;
//States definiëren
-enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_OFFSET_BICEPS, EMG_BICEPS, KALIBRATIE_BICEPS, EMG_OFFSET_TRICEPS, EMG_TRICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
+enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
uint8_t state = RUST; //State is rust aan het begin
//Gedefinieerde datatypen en naamgeving en beginwaarden
@@ -205,8 +206,8 @@
pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);
if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
- state = EMG_OFFSET_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM2 afgerond\n"); //Tekst voor controle Arm 2 naar thuispositie
+ state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
+ pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
}
}
@@ -221,13 +222,12 @@
xbhp3 = xbhp2;
xbhp2 = xbhp1;
xbhp1 = xbhp;
- xbn = xbhp;
ybhp4 = ybhp3;
ybhp3 = ybhp2;
ybhp2 = ybhp1;
ybhp1 = ybhp;
- ybn1 = ybhp; //De vorige waarde in het notchfilter is de waarde uit het high pass filter
+ xbn = ybhp; //De input in het notchfilter is de waarde uit het high pass filter
//Notch
ybn = -C2*ybn1 - C3*ybn2 + D1*xbn + D2*xbn1 + D3*xbn2; //Filterfunctie in z-domein
@@ -235,16 +235,15 @@
//Waarden voor de volgende ronde benoemen
xbn2 = xbn1;
xbn1 = xbn;
- xblp = xbn;
ybn2 = ybn1;
ybn1 = ybn;
- rb = ybn;
+ rb = ybn; //De input in de rectify is de waarde uit het notch filter
//Rectify
rbr = fabs(rb); //Absolute waarde van de waarde uit notchfilter
- yblp1 = rbr;
+ xblp = rbr; //De input in het low pass filter is de waarde uit de rectify
//Low pass
yblp = -E2*yblp1 - E3*yblp2 - E4*yblp3 - E5*yblp4 + F1*xblp + F2*xblp1 + F3*xblp2 + F4*xblp3 + F5*xblp4; //Filterfunctie in z-domein
@@ -253,13 +252,12 @@
xblp3 = xblp2;
xblp2 = xblp1;
xblp1 = xblp;
- xbf = xblp;
yblp4 = yblp3;
yblp3 = yblp2;
yblp2 = yblp1;
yblp1 = yblp;
- ybf = yblp;
+ xbf = yblp; //De uiteindelijk gefilterde EMG waarde is de output uit het low pass filter
}
@@ -275,14 +273,15 @@
lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
wait(2); //Twee seconden wachten
- pc.printf("RUST afgerond\n"); //Tekst voor controle Aanzetten
+ pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
break; //Stopt acties in deze case
}
case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
- pc.printf("KALIBRATIE_ARM1\n");
+ pc.printf("KALIBRATIE_ARM1\n\r");
wait(1); //Een seconde wachten
+
ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
while(state == KALIBRATIE_ARM1) {
@@ -296,7 +295,7 @@
}
case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
- pc.printf("KALIBRATIE_ARM1\n");
+ pc.printf("KALIBRATIE_ARM1\n\r");
wait(1); //Een seconde wachten
//ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
@@ -311,67 +310,47 @@
ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
break; //Stopt acties in deze case
}
-
- case EMG_OFFSET_BICEPS: { //Kalibratie EMG signaal biceps
- pc.printf("EMG_OFFSET_BICEPS\n");
- wait(1); //Een seconde wachten
-
- ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
-
- pc.printf("Ticker voor kalibratie compleet\n"); //Tekst voor controle Ticker voor kalibratie
-
- //5 seconden EMG biceps meten
-
- wait(1); //Een seconde wachten
- lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
- lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
-
- while(state == EMG_OFFSET_BICEPS){
+
+ case EMG_KALIBRATIE_BICEPS:
+ {
+ pc.printf("EMG__KALIBRATIE_BICEPS\n\r");
+
+ lcd_r1 = " SPAN IN 5 SEC. ";
+ lcd_r2 = " 2 X BICEPS AAN ";
+
+ ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
+ biceps_kalibratie.start();
+
+ if(biceps_kalibratie.read() <= 5){
while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
- regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- pc.printf("Ga EMG meten\n");
-
- xbk = EMG_bi.read(); //EMG signaal uitlezen
- xbk10 = xbk9;
- xbk9 = xbk8;
- xbk8 = xbk7;
- xbk7 = xbk6;
- xbk6 = xbk5;
- xbk5 = xbk4;
- xbk4 = xbk3;
- xbk3 = xbk2;
- xbk2 = xbk1;
- xbk1 = xbk;
- pc.printf("xbk10 is %f\n", xbk10);
+ xb = EMG_bi.read(); //EMG meten van biceps
+
+ filter_biceps();
- if(xbk10 > 0){
- pc.printf("10 waarden gemeten\n");
- state = EMG_BICEPS;
- }
+ if(biceps_kalibratie.read() == 1){
+ lcd_r1 = " 4 ";
+ }
+ if(biceps_kalibratie.read() == 2){
+ lcd_r1 = " 3 ";
+ }
+ if(biceps_kalibratie.read() == 3){
+ lcd_r1 = " 2 ";
+ }
+ if(biceps_kalibratie.read() == 4){
+ lcd_r1 = " 1 ";
+ }
+
}
- break;
- }
-
- case EMG_BICEPS:
- {
- pc.printf("EMG_BICEPS\n");
- meanxbk = (xbk1 + xbk2 + xbk3 + xbk4 + xbk5 + xbk6 + xbk7 + xbk8 + xbk9 + xbk10)/10; //Offset bepalen van de eerste 10 gemeten waarden
- while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
- regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
-
- xb = EMG_bi.read(); //EMG meten van biceps
- xbo = xb - meanxbk; //Gemeten waarden zonder offset
-
- filter_biceps();
-
+ state = EMG_KALIBRATIE_TRICEPS;
}
default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
}
}
- pc.printf("state: %u\n",state);
+ pc.printf("state: %u\n\r",state);
}
}
\ No newline at end of file