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:
- 27:5ac1fc1005d7
- Parent:
- 26:438a498e5526
- Child:
- 28:e2f5cee5e73b
--- a/main.cpp Mon Nov 03 13:00:39 2014 +0000 +++ b/main.cpp Mon Nov 03 16:09:53 2014 +0000 @@ -7,7 +7,7 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten -#define KP_arm1 0.02 //Factor proprotionele regelaar arm 1 +#define KP_arm1 0.025 //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 @@ -343,7 +343,7 @@ } } - xbt = xbfmax * 0.90; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten + xbt = xbfmax * 0.80; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten pc.printf("maximale biceps %f", xbfmax); //Maximale gefilterde EMG waarde naar pc sturen pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps @@ -406,7 +406,7 @@ } } - xtt = xtfmax * 0.90; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten + xtt = xtfmax * 0.80; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten pc.printf("maximale triceps %f", xtfmax); //Maximale gefilterde EMG waarde naar pc sturen pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen state = START; //Gaat door naar het slaan, kalibratie nu afgerond @@ -419,6 +419,15 @@ pc.printf("START\n\r"); //Controle naar pc sturen + EMG.start(); //Timer aanzetten voor EMG meten + + while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten + 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 + + EMG_meten(); //EMG meten van biceps en triceps + } + while(state == START) { 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 @@ -443,6 +452,7 @@ lcd.printf(" B "); //Tekst op LCD scherm pc.printf("B\n\r"); //Controle naar pc sturen + EMG.reset(); EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten @@ -473,6 +483,7 @@ lcd.printf(" T "); //Tekst op LCD scherm pc.printf("T\n\r"); //Controle naar pc sturen + EMG.reset(); EMG.start(); //Timer aanzetten voor EMG meten while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten @@ -886,6 +897,10 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= -84){ + referentie_arm1 = -84; + } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde @@ -898,6 +913,7 @@ referentie_arm2 = -0.5 * 100000 * t * t; //Referentie arm 2 loopt parabolisch af t = t + 0.005; //Tijd loopt op met 0.005 per flag + pwm_motor_arm1 = 0.05; pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen pc.printf("get position %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen @@ -955,6 +971,10 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= -84){ + referentie_arm1 = -84; + } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde @@ -1002,11 +1022,15 @@ regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan while(puls_motor_arm1.getPosition() > -84) { - referentie_arm1 = referentie_arm1 - 264.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde + referentie_arm1 = referentie_arm1 - 259.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= -84){ + referentie_arm1 = -84; + } while(puls_motor_arm2.getPosition() < 211) { referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde @@ -1123,6 +1147,10 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= 48){ + referentie_arm1 = 48; + } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde @@ -1191,6 +1219,10 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= 48){ + referentie_arm1 = 48; + } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie @@ -1243,6 +1275,10 @@ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen } + + if(puls_motor_arm1.getPosition() <= 48){ + referentie_arm1 = 48; + } while(puls_motor_arm2.getPosition() < 167) { referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie @@ -1403,6 +1439,7 @@ } } } + break; } case THUISPOSITIE_MIDDEN: { //Terug naar gekalibreerde positie @@ -1441,15 +1478,17 @@ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen } - while(puls_motor_arm1.getPosition() < 180) { + while(puls_motor_arm1.getPosition() < 175) { 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 - referentie_arm1 = referentie_arm1 + 264.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde + referentie_arm1 = referentie_arm1 + 259.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen } + state = WACHT; + break; } case WACHT: { //Even wachten en weer terug naar start @@ -1457,6 +1496,7 @@ lcd.printf(" WACHT "); //Tekst op LCD scherm wait(3); //Drie seconden wachten state = START; //Terug naar state START + break; } default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case @@ -1466,3 +1506,4 @@ } } +