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:
- 8:d4161e9be1da
- Parent:
- 7:dd3cba61b34b
--- a/main.cpp Fri Oct 17 07:52:22 2014 +0000 +++ b/main.cpp Mon Oct 20 11:56:38 2014 +0000 @@ -5,19 +5,19 @@ //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 -TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm +TextLCD lcd(PTD3, 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 +PwmOut pwm_motor_arm1(PTC8); //PWM naar motor arm 1 +DigitalOut dir_motor_arm1(PTC9); //Richting van motor arm 1 +Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1 +PwmOut pwm_motor_arm2(PTA5); //PWM naar motor arm 2 +DigitalOut dir_motor_arm2(PTA4); //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 @@ -68,14 +68,15 @@ 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); + 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 + pc.printf("pwm_to_motor_arm1 is %d\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) + 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 } @@ -92,6 +93,7 @@ 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 + pc.printf("pwm_to_motor_arm2 is %f\n\r",puls_motor_arm2.getPosition()); 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; @@ -130,7 +132,7 @@ } wait(1); //Een seconde wachten } - + pc.printf("hoi"); //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