Janine Klein Rot
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 @@
}
}
+