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:
- 13:54ee98850a15
- Parent:
- 12:996f9f8e3acc
- Child:
- 14:e1816efa712d
--- a/main.cpp Thu Oct 23 11:46:37 2014 +0000
+++ b/main.cpp Fri Oct 24 11:06:15 2014 +0000
@@ -5,11 +5,48 @@
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP 0.001 //Factor proprotionele regelaar
+#define KP_arm1 0.3 //Factor proprotionele regelaar arm 1
+#define KI_arm1 0 //Factor integraal regelaar arm 1
+#define KD_arm1 0.02 //Factor afgeleide regelaar arm 1
+#define KP_arm2 0.001 //Factor proprotionele regelaar arm 2
+#define KI_arm2 0 //Factor integraal regelaar arm 2
+#define KD_arm2 0 //Factor afgeleide regelaar arm 2
#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
#define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
#define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
+//High Pass filter Filtercoëfficiënten
+#define A1 1
+#define A2 -3.1806
+#define A3 3.8612
+#define A4 -2.1122
+#define A5 0.4383
+#define B1 4.1660E-4
+#define B2 0.0017
+#define B3 0.0025
+#define B4 0.0017
+#define B5 4.1660E-4
+
+//Notch filter Filtercoëfficiënten
+#define C1 1
+#define C2 -1.1873E-16
+#define C3 0.9391
+#define D1 0.9695
+#define D2 -1.1873E-16
+#define D3 0.9695
+
+//Low pass filter Filtercoëfficiënten
+#define E1 1
+#define E2 3.9179
+#define E3 5.7571
+#define E4 3.7603
+#define E5 0.9212
+#define F1 0.9598
+#define F2 3.8391
+#define F3 5.7587
+#define F4 3.8391
+#define F5 0.9598
+
//Pinverdeling en naamgeving variabelen
TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
MODSERIAL pc(USBTX, USBRX); //PC
@@ -27,7 +64,7 @@
Ticker ticker_EMG; //Ticker voor EMG meten
//States definiëren
-enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
+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
uint8_t state = RUST; //State is rust aan het begin
//Gedefinieerde datatypen en naamgeving en beginwaarden
@@ -36,8 +73,72 @@
float pwm_to_motor_arm1; //PWM output naar motor arm 1
float pwm_to_motor_arm2; //PWM output naar motor arm 2
-float xbk; //Gemeten EMG waarde biceps in de kalibratie
-int i; //Voor for-loop
+float error_arm1_kalibratie; //Error in pulsen arm 1
+float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
+float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
+float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
+float error_arm2_kalibratie; //Error in pulsen arm 2
+float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
+float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
+float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
+float xbk = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk1 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk2 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk3 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk4 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk5 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk6 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk7 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk8 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk9 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float xbk10 = 0; //Gemeten EMG waarde biceps in de kalibratie
+float meanxbk; //Offset van biceps kalibratie
+float xb; //Gemeten EMG waarde biceps
+float xbo; //Gemeten EMG waarde biceps min de offset, deze gaat in de filters
+
+//High Pass filter Y(n-x) waarden instellen op nul en definiëren als float
+float xbhp; //Zelfde als xbo, maar makkelijker in notatie in filter
+float xbhp1 = 0;
+float xbhp2 = 0;
+float xbhp3 = 0;
+float xbhp4 = 0;
+float xbhp5 = 0;
+float ybhp;
+float ybhp1 = 0;
+float ybhp2 = 0;
+float ybhp3 = 0;
+float ybhp4 = 0;
+float ybhp5 = 0;
+
+//Notch filter
+float xbn;
+float xbn1 = 0;
+float xbn2 = 0;
+float ybn;
+float ybn1 = 0;
+float ybn2 = 0;
+
+//Rectifier
+float rb;
+float rbr;
+
+//Low pass filter
+float xblp;
+float xblp1 = 0;
+float xblp2 = 0;
+float xblp3 = 0;
+float xblp4 = 0;
+float xblp5 = 0;
+float yblp;
+float yblp1 = 0;
+float yblp2 = 0;
+float yblp3 = 0;
+float yblp4 = 0;
+float yblp5 = 0;
+
+//Gefilterde biceps EMG
+float xbf;
+float ybf;
volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
@@ -65,7 +166,10 @@
void arm1_naar_thuispositie()
{
- pwm_to_motor_arm1 = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1; //Output naar motor na PID regelaar
keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
@@ -79,13 +183,16 @@
if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie
+ pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
}
}
void arm2_naar_thuispositie()
{
- pwm_to_motor_arm2 = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2; //Output naar motor na PID regelaar
keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
@@ -94,13 +201,67 @@
dir_motor_arm2.write(0);
}
pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
+ pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
+ 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 = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie
+ 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
}
}
+void filter_biceps()
+{
+ //High pass
+ xbhp = xbo; //Input in filter
+ ybhp = -A2*ybhp1 - A3*ybhp2 - A4*ybhp3 - A5*ybhp4 + B1*xbhp + B2*xbhp1 + B3*xbhp2 + B4*xbhp3 + B5*xbhp4; //Filterformule in z-domein
+
+ //Waarden voor de volgende ronde benoemen
+ xbhp4 = xbhp3;
+ 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
+
+ //Notch
+ ybn = -C2*ybn1 - C3*ybn2 + D1*xbn + D2*xbn1 + D3*xbn2; //Filterfunctie in z-domein
+
+ //Waarden voor de volgende ronde benoemen
+ xbn2 = xbn1;
+ xbn1 = xbn;
+ xblp = xbn;
+
+ ybn2 = ybn1;
+ ybn1 = ybn;
+ rb = ybn;
+
+ //Rectify
+ rbr = fabs(rb); //Absolute waarde van de waarde uit notchfilter
+
+ yblp1 = rbr;
+
+ //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
+
+ xblp4 = xblp3;
+ xblp3 = xblp2;
+ xblp2 = xblp1;
+ xblp1 = xblp;
+ xbf = xblp;
+
+ yblp4 = yblp3;
+ yblp3 = yblp2;
+ yblp2 = yblp1;
+ yblp1 = yblp;
+ ybf = yblp;
+}
+
int main()
{
@@ -114,13 +275,13 @@
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"); //Tekst voor controle Aanzetten
+ pc.printf("RUST afgerond\n"); //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");
+ pc.printf("KALIBRATIE_ARM1\n");
wait(1); //Een seconde wachten
ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
@@ -135,6 +296,7 @@
}
case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
+ pc.printf("KALIBRATIE_ARM1\n");
wait(1); //Een seconde wachten
//ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
@@ -150,12 +312,13 @@
break; //Stopt acties in deze case
}
- case KALIBRATIE_BICEPS: { //Kalibratie EMG signaal biceps
+ 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"); //Tekst voor controle Ticker voor kalibratie
+ pc.printf("Ticker voor kalibratie compleet\n"); //Tekst voor controle Ticker voor kalibratie
//5 seconden EMG biceps meten
@@ -163,15 +326,48 @@
lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
- for (i=0; i<1000; i++) {
+ while(state == EMG_OFFSET_BICEPS){
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
+
+ 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);
+
+ if(xbk10 > 0){
+ pc.printf("10 waarden gemeten\n");
+ state = EMG_BICEPS;
+ }
}
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();
}
+
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
}