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-17
- Revision:
- 7:dd3cba61b34b
- Parent:
- 6:3b6fad529520
- Child:
- 8:d4161e9be1da
- Child:
- 9:454e7da8ab65
File content as of revision 7:dd3cba61b34b:
#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 //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor #define KP 1 //Factor proprotionele regelaar #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten //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(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 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps Ticker ticker_regelaar; //Ticker voor regelaar motor Ticker ticker_EMG; //Ticker voor EMG meten //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 int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is 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; // 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(){ pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor 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(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM 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 } } void arm2_naar_thuispositie(){ pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor 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(abs(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 } } int main() { //PC baud rate instellen pc.baud(38400); //PC baud rate is 38400 bits/seconde //Aanzetten if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ 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 } //Arm 1 naar thuispositie if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ 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(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 } //Arm 2 naar thuispositie if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ 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(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 voor kalibratie if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ 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 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 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan xbk = EMG_bi.read(); //EMG signaal uitlezen } } }