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:
- 9:454e7da8ab65
- Parent:
- 7:dd3cba61b34b
- Child:
- 10:52b22bff409a
--- a/main.cpp Fri Oct 17 07:52:22 2014 +0000 +++ b/main.cpp Mon Oct 20 20:06:06 2014 +0000 @@ -5,7 +5,7 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor -#define KP 1 //Factor proprotionele regelaar +#define KP 0.001 //Factor proprotionele regelaar #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten //Pinverdeling en naamgeving variabelen @@ -13,24 +13,22 @@ MODSERIAL pc(USBTX, USBRX); //PC PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 -DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1 -Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1 -PwmOut pwm_motor_arm2(PTA12); //PWM naar motor arm 2 -DigitalOut dir_motor_arm2(PTD3); //Ricting van motor arm 2 -Encoder puls_motor_arm2(PTD4, PTC8); //Encoder pulsen tellen van motor arm 2 +DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 +Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1 +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 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps Ticker ticker_regelaar; //Ticker voor regelaar motor 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 +uint8_t state = RUST; //State is rust aan het begin + //Gedefinieerde datatypen en naamgeving en beginwaarden -bool rust = false; //Bool voor controleren volgorde in programma -bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma -bool kalibratie_positie_arm2 = false; //Bool voor controleren volgorde in programma -bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma -bool kalibratie_EMG_tri = false; //Bool voor controleren volgorde in programma - 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 @@ -73,11 +71,13 @@ else { //Anders richting nul dir_motor_arm1.write(0); } - pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM + pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM + pc.printf("pulsmotorgetposition %f\n\r", 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) - kalibratie_positie_arm1 = true; - pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie + 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 } } @@ -91,11 +91,11 @@ else { //Anders richting nul dir_motor_arm2.write(0); } - pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM + pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM 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) - kalibratie_positie_arm2 = true; - pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie + 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 } } @@ -104,53 +104,59 @@ //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 + { - //Aanzetten - if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ + 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("Aanzetten compleet"); //Tekst voor controle Aanzetten - rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel + pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten + state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel + break; //Stopt acties in deze case } - - - //Arm 1 naar thuispositie - if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ + case KALIBRATIE_ARM1: //Arm 1 naar thuispositie + { wait(1); //Een seconde wachten ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken - while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { + 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 + } + wait(1); //Een seconde wachten + break; //Stopt acties in deze case } - //Arm 2 naar thuispositie - if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ + case KALIBRATIE_ARM2: //Arm 2 naar thuispositie + { wait(1); //Een seconde wachten //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken - while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { + 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 + ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer + break; //Stopt acties in deze case } - //Ticker voor kalibratie - if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ + case KALIBRATIE_BICEPS: //Kalibratie EMG signaal biceps + { + wait(1); //Een seconde wachten + + ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken - 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 //5 seconden EMG biceps meten @@ -158,7 +164,6 @@ 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 - wait(2); //Twee seconden wachten for (i=0; i<1000; i++){ while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel @@ -170,6 +175,9 @@ } + } + + } \ No newline at end of file