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:
- 11:e9b906b5f572
- Parent:
- 10:52b22bff409a
- Child:
- 12:996f9f8e3acc
diff -r 52b22bff409a -r e9b906b5f572 main.cpp --- a/main.cpp Tue Oct 21 11:57:27 2014 +0000 +++ b/main.cpp Tue Oct 21 14:51:23 2014 +0000 @@ -4,9 +4,11 @@ #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 0.001 //Factor proprotionele regelaar -#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten +#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor +#define KP 0.001 //Factor proprotionele regelaar +#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 //Pinverdeling en naamgeving variabelen TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm @@ -15,12 +17,11 @@ PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 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 +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 +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 @@ -32,12 +33,10 @@ 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; // +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 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,52 +64,52 @@ 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 + 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 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 + if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 dir_motor_arm1.write(1); - } else { //Anders richting nul + } 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 + 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"); //Tekst voor controle Arm 1 naar thuispositie + 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 } } 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 + 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 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 + if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 dir_motor_arm2.write(1); - } else { //Anders richting nul + } 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 + 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) - 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 + 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 } } int main() { - while(1) { + 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 + 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 @@ -131,10 +130,10 @@ arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen } wait(1); //Een seconde wachten - break; //Stopt acties in deze case + break; //Stopt acties in deze case } - case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie + 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 @@ -145,8 +144,8 @@ 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 + wait(1); //Een seconde wachten + ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer break; //Stopt acties in deze case } @@ -172,8 +171,8 @@ break; } - default: { - state = RUST; + 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",state);