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:
- 30:f79cf70e2917
- Parent:
- 29:f95f0cc84349
- Child:
- 31:36fe36657f8d
diff -r f95f0cc84349 -r f79cf70e2917 main.cpp
--- a/main.cpp Mon Nov 03 23:03:59 2014 +0000
+++ b/main.cpp Tue Nov 04 00:29:03 2014 +0000
@@ -448,8 +448,6 @@
}
case B: { //Tweede keuze maken voor doel
- state = BBBB;
- break;
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" B "); //Tekst op LCD scherm
pc.printf("B\n\r"); //Controle naar pc sturen
@@ -904,8 +902,6 @@
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
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
@@ -985,32 +981,30 @@
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
}
-
+ ticker_motor_arm2_pid.detach();
+ pwm_to_motor_arm2 = 1;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(0);
while(puls_motor_arm2.getPosition() > 36) {
- referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch af
- t = t + 0.005; //Tijd loopt op met 0.005 per flag
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
pc.printf("t is %f\n\r", t); //T naar pc sturen
}
-
- if(puls_motor_arm2.getPosition() <= 36) {
- wait(1); //Een seconde wachten
- while(puls_motor_arm2.getPosition() < 211) {
- referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
+ pwm_to_motor_arm2 = 0.5;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(1);
+ while(puls_motor_arm2.getPosition() <= 211) {
- pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
- pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
- }
+ pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
+ }
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+ state = THUISPOSITIE_RECHTS; //Door naar de volgende state
- if(puls_motor_arm2.getPosition() >= 211) {
- pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
- state = THUISPOSITIE_RECHTS; //Door naar de volgende state
- }
- }
+
}
break; //Stop met alle acties in deze case
}
@@ -1041,33 +1035,31 @@
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
}
-
+ ticker_motor_arm2_pid.detach();
+ pwm_to_motor_arm2 = 1;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(0);
while(puls_motor_arm2.getPosition() > 36) {
- referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af
- t = t + 0.005; //Tijd loopt op met 0.005 iedere flag
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
pc.printf("t is %f\n\r", t); //T naar pc sturen
}
-
- if(puls_motor_arm2.getPosition() <= 36) {
- wait(1); //Een seconde wachten
- while(puls_motor_arm2.getPosition() < 211) {
- referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
+ pwm_to_motor_arm2 = 0.5;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(1);
+ while(puls_motor_arm2.getPosition() <= 211) {
- pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
- pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
- }
+ pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
+ }
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+ pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
+ state = THUISPOSITIE_RECHTS; //Door naar de volgende state
+ }
- if(puls_motor_arm2.getPosition() >= 211) {
- pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
- state = THUISPOSITIE_RECHTS; //Door naar de volgende state
- }
- }
- }
break; //Stop met alle acties in deze case
}
@@ -1161,32 +1153,31 @@
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
}
+ ticker_motor_arm2_pid.detach();
+ pwm_to_motor_arm2 = 1;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(0);
while(puls_motor_arm2.getPosition() > -8) {
- 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
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
pc.printf("t is %f\n\r", t); //T naar pc sturen
}
-
- if(puls_motor_arm2.getPosition() <= -8) {
- wait(1); //Een seconde wachten
- while(puls_motor_arm2.getPosition() < 167) {
- referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
+ pwm_to_motor_arm2 = 0.5;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(1);
- pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
- pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
- }
+ while(puls_motor_arm2.getPosition() <= 167) {
+ pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
+ }
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+ pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
+ state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- if(puls_motor_arm2.getPosition() >= 167) {
- pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
- state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- }
- }
}
break; //Stop met alle acties in deze case
}
@@ -1233,32 +1224,32 @@
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
}
+ ticker_motor_arm2_pid.detach();
+ pwm_to_motor_arm2 = 1;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(0);
while(puls_motor_arm2.getPosition() > -8) {
- referentie_arm2 = -0.5 * 150000 * t * t; //Referentie arm 2 loopt parabolisch op
- t = t + 0.005; //Tijd loopt op met 0.005 per flag
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
pc.printf("t is %f\n\r", t); //T naar pc sturen
}
-
- if(puls_motor_arm2.getPosition() <= -8) {
- wait(1); //Een seconde wachten
- while(puls_motor_arm2.getPosition() < 167) {
- referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie
+ pwm_to_motor_arm2 = 0.5;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(1);
- pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
- pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
- }
+ while(puls_motor_arm2.getPosition() <= 167) {
+ pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
+ }
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+ pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
+ state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- if(puls_motor_arm2.getPosition() >= 167) {
- pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
- state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- }
- }
+
}
break; //Stop met alle acties in deze case
}
@@ -1289,32 +1280,32 @@
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen
}
+ ticker_motor_arm2_pid.detach();
+ pwm_to_motor_arm2 = 1;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(0);
while(puls_motor_arm2.getPosition() > -8) {
- referentie_arm2 = -0.5 * 200000 * t * t; //Referentie arm 2 loopt parabolisch af
- t = t + 0.005; //Tijd loopt op met 0.005 per flag
pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen
pc.printf("t is %f\n\r", t); //T naar pc sturen
}
-
- if(puls_motor_arm2.getPosition() <= -8) {
- wait(1); //Een seconde wachten
- while(puls_motor_arm2.getPosition() < 167) {
- referentie_arm2 = referentie_arm2 + 175.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde
+ pwm_to_motor_arm2 = 0.5;
+ pwm_motor_arm2.write(pwm_to_motor_arm2);
+ dir_motor_arm2.write(1);
- pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
- pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
- pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
- }
+ while(puls_motor_arm2.getPosition() <= 167) {
+ pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen
+ }
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+ pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
+ state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- if(puls_motor_arm2.getPosition() >= 167) {
- pc.printf("staat stil\n\r"); //Staat stil naar pc sturen
- state = THUISPOSITIE_MIDDEN; //Door naar de volgende state
- }
- }
+
}
break; //Stop met alle acties in deze case
}