2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp
- Committer:
- JKleinRot
- Date:
- 2014-10-28
- Revision:
- 15:3ebd0e666a9c
- Parent:
- 14:e1816efa712d
- Child:
- 16:c34c5d9dfe1a
File content as of revision 15:3ebd0e666a9c:
#include "mbed.h" //Mbed bibliotheek inladen, standaard functies #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.01 //Factor proprotionele regelaar arm 1 #define KI_arm1 0 //Factor integraal regelaar arm 1 #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 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 -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 #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 -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 MODSERIAL pc(USBTX, USBRX); //PC PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit) PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 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, 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 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm float pwm_to_motor_arm1; //PWM output naar motor arm 1 float pwm_to_motor_arm2; //PWM output naar motor arm 2 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 xb; //Gemeten EMG waarde biceps float xt; arm_biquad_casd_df1_inst_f32 highpass_biceps; float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; float highpass_biceps_states[4]; 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; arm_biquad_casd_df1_inst_f32 highpass_triceps; float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; float highpass_triceps_states[4]; arm_biquad_casd_df1_inst_f32 notch_triceps; float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; float notch_triceps_states[4]; arm_biquad_casd_df1_inst_f32 lowpass_triceps; float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; float lowpass_triceps_states[4]; 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 { regelaar_ticker_flag = true; } volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true { regelaar_EMG_flag = true; } void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt { if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum *in = min; } if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum *in = max; } else { //In alle andere gevallen is de waarde de waarde zelf *in = *in; } } void arm1_naar_thuispositie() { 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 dir_motor_arm1.write(1); } else { //Anders richting nul dir_motor_arm1.write(0); } pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); 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\n"); //Tekst voor controle Arm 1 naar thuispositie } } void arm2_naar_thuispositie() { 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 dir_motor_arm2.write(1); } else { //Anders richting nul 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 = 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 } } void filter_biceps() { 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); 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() { while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan //PC baud rate instellen pc.baud(38400); //PC baud rate is 38400 bits/seconde switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case case RUST: { //Aanzetten 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\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\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) { while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen } wait(1); //Een seconde wachten break; //Stopt acties in deze case } case KALIBRATIE_ARM2: { //Arm 2 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_ARM2) { while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen } wait(1); //Een seconde wachten ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } 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(); while(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(); if(int(biceps_kalibratie.read()) == 1) { lcd_r1 = " 4 "; pc.printf("4"); } if(biceps_kalibratie.read() == 2) { lcd_r1 = " 3 "; pc.printf("3"); } if(biceps_kalibratie.read() == 3) { lcd_r1 = " 2 "; pc.printf("2"); } 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; } xtt = xtfmax * 0.8; pc.printf("threshold is %f\n\r", xtt); state = BEGIN_METEN; break; } 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); } } }