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:
- 15:3ebd0e666a9c
- Parent:
- 14:e1816efa712d
- Child:
- 16:c34c5d9dfe1a
--- a/main.cpp Mon Oct 27 14:43:50 2014 +0000
+++ b/main.cpp Tue Oct 28 11:11:13 2014 +0000
@@ -2,30 +2,27 @@
#include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
#include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
#include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
+#include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP_arm1 0.3 //Factor proprotionele regelaar arm 1
+#define KP_arm1 0.01 //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 KD_arm1 0 //Factor afgeleide regelaar arm 1
+#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
#define KI_arm2 0 //Factor integraal regelaar arm 2
-#define KD_arm2 0 //Factor afgeleide 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
+#define A1 1
+#define A2 -1.5610
+#define A3 0.6414
+#define B1 0.0201
+#define B2 0.0402
+#define B3 0.0201
//Notch filter Filtercoëfficiënten
#define C1 1
@@ -37,15 +34,12 @@
//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
+#define E2 -1.9645
+#define E3 0.9651
+#define F1 1.5515E-4
+#define F2 3.1030E-4
+#define F3 1.5515E-4
+
//Pinverdeling en naamgeving variabelen
TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
@@ -58,14 +52,16 @@
DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
+AnalogIn EMG_tri(PTB2);
//Blauw op 3,3 V en groen op GND
Ticker ticker_regelaar; //Ticker voor regelaar motor
Ticker ticker_EMG; //Ticker voor EMG meten
Timer biceps_kalibratie;
+Timer triceps_kalibratie;
//States definiëren
-enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_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, BEGIN_METEN, B, T}; //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
@@ -82,64 +78,41 @@
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
+float xt;
+
+arm_biquad_casd_df1_inst_f32 highpass_biceps;
+float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3};
+float highpass_biceps_states[4];
-//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;
+arm_biquad_casd_df1_inst_f32 notch_biceps;
+float notch_biceps_const[] = {D1, D2, D3, -C2, -C3};
+float notch_biceps_states[4];
+
+arm_biquad_casd_df1_inst_f32 lowpass_biceps;
+float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3};
+float lowpass_biceps_states[4];
+
+float xbf;
+float xbfmax = 0;
-//Notch filter
-float xbn;
-float xbn1 = 0;
-float xbn2 = 0;
-float ybn;
-float ybn1 = 0;
-float ybn2 = 0;
+arm_biquad_casd_df1_inst_f32 highpass_triceps;
+float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3};
+float highpass_triceps_states[4];
-//Rectifier
-float rb;
-float rbr;
+arm_biquad_casd_df1_inst_f32 notch_triceps;
+float notch_triceps_const[] = {D1, D2, D3, -C2, -C3};
+float notch_triceps_states[4];
-//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;
+arm_biquad_casd_df1_inst_f32 lowpass_triceps;
+float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3};
+float lowpass_triceps_states[4];
-//Gefilterde biceps EMG
-float xbf;
-float ybf;
+float xtf;
+float xtfmax = 0;
+
+float xbt;
+float xtt;
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
@@ -213,53 +186,32 @@
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;
-
- ybhp4 = ybhp3;
- ybhp3 = ybhp2;
- ybhp2 = ybhp1;
- ybhp1 = ybhp;
- 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
-
- //Waarden voor de volgende ronde benoemen
- xbn2 = xbn1;
- xbn1 = xbn;
+ arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1);
+
+ arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1);
+
+ xbf = fabs(xbf);
+
+ arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1);
- ybn2 = ybn1;
- ybn1 = 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
-
- 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
-
- xblp4 = xblp3;
- xblp3 = xblp2;
- xblp2 = xblp1;
- xblp1 = xblp;
-
- yblp4 = yblp3;
- yblp3 = yblp2;
- yblp2 = yblp1;
- yblp1 = yblp;
- xbf = yblp; //De uiteindelijk gefilterde EMG waarde is de output uit het low pass filter
+ pc.printf("xbf is %f\n\r", xbf);
+
+
}
+void filter_triceps()
+{
+ arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1);
+
+ arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1);
+
+ xtf = fabs(xtf);
+
+ arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1);
+
+ pc.printf("xtf is %f\n\r", xtf);
+
+}
int main()
{
@@ -281,7 +233,7 @@
case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
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) {
@@ -310,47 +262,137 @@
ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
break; //Stopt acties in deze case
}
-
- case EMG_KALIBRATIE_BICEPS:
- {
+
+ case EMG_KALIBRATIE_BICEPS: {
pc.printf("EMG__KALIBRATIE_BICEPS\n\r");
-
+
lcd_r1 = " SPAN IN 5 SEC. ";
lcd_r2 = " 2 X BICEPS AAN ";
-
+ pc.printf("span biceps aan\n\r");
+ pc.printf("EMG biceps %f\n\r",xb);
+
+ arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states);
+ arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states);
+ arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states);
+
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
-
- xb = EMG_bi.read(); //EMG meten van biceps
-
- filter_biceps();
+
+ while(biceps_kalibratie.read() <= 5) {
- if(biceps_kalibratie.read() == 1){
+ 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
+
+ filter_biceps();
+
+ if(int(biceps_kalibratie.read()) == 1) {
lcd_r1 = " 4 ";
+ pc.printf("4");
}
- if(biceps_kalibratie.read() == 2){
+ if(biceps_kalibratie.read() == 2) {
lcd_r1 = " 3 ";
+ pc.printf("3");
}
- if(biceps_kalibratie.read() == 3){
+ if(biceps_kalibratie.read() == 3) {
lcd_r1 = " 2 ";
+ pc.printf("2");
}
- if(biceps_kalibratie.read() == 4){
+ if(biceps_kalibratie.read() == 4) {
+ lcd_r1 = " 1 ";
+ pc.printf("1");
+ }
+
+ }
+ if(xbf >= xbfmax) {
+ xbfmax = xbf;
+ }
+ xbt = xbfmax * 0.8;
+ pc.printf("threshold is %f\n\r", xbt);
+ state = EMG_KALIBRATIE_TRICEPS;
+ break;
+ }
+
+ case EMG_KALIBRATIE_TRICEPS: {
+ pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");
+
+ lcd_r1 = " SPAN IN 5 SEC. ";
+ lcd_r2 = " 2 X TRICEPS AAN";
+ pc.printf("span triceps aan\n\r");
+ pc.printf("EMG biceps %f\n\r",xt);
+
+ arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states);
+ arm_biquad_cascade_df1_init_f32(¬ch_triceps, 1, notch_triceps_const, notch_triceps_states);
+ arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states);
+
+ ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
+ triceps_kalibratie.start();
+
+ while(triceps_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
+
+ xt = EMG_tri.read(); //EMG meten van biceps
+
+ filter_triceps();
+
+ if(triceps_kalibratie.read() == 1) {
+ lcd_r1 = " 4 ";
+ pc.printf("4");
+ }
+ if(triceps_kalibratie.read() == 2) {
+ lcd_r1 = " 3 ";
+ pc.printf("3");
+ }
+ if(triceps_kalibratie.read() == 3) {
+ lcd_r1 = " 2 ";
+ pc.printf("2");
+ }
+ if(triceps_kalibratie.read() == 4) {
lcd_r1 = " 1 ";
}
-
+
+ }
+ if(xtf >= xtfmax) {
+ xtfmax = xtf;
}
-
- state = EMG_KALIBRATIE_TRICEPS;
+ xtt = xtfmax * 0.8;
+ pc.printf("threshold is %f\n\r", xtt);
+ state = BEGIN_METEN;
+ break;
}
-
- 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
+
+ case BEGIN_METEN: {
+ lcd_r1 = " BEGIN ";
+ pc.printf("KEUZE1\n\r");
+
+ while(state == BEGIN_METEN) {
+ 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;
+
+ xb = EMG_bi.read();
+ filter_biceps();
+ xt = EMG_tri.read();
+ filter_triceps();
+
+ if(xb >= xbt) {
+ state = B;
+ }
+ if(xt >= xtt) {
+ state = T;
+ }
+ }
+
+
+
+
+ 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\r",state);
}
- pc.printf("state: %u\n\r",state);
}
}
\ No newline at end of file