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:
- 29:f95f0cc84349
- Parent:
- 28:e2f5cee5e73b
- Child:
- 30:f79cf70e2917
--- a/main.cpp Mon Nov 03 21:12:14 2014 +0000
+++ b/main.cpp Mon Nov 03 23:03:59 2014 +0000
@@ -12,7 +12,7 @@
#define KD_arm1 0 //Factor afgeleide regelaar arm 1
#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
#define KI_arm2 0 //Factor integraal regelaar arm 2
-#define KD_arm2 0 //Factor afgeleide regelaar arm 2
+#define KD_arm2 0.0008 //Factor afgeleide regelaar arm 2
//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
//High Pass filter
@@ -448,6 +448,8 @@
}
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
@@ -897,46 +899,47 @@
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){
+
+ 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
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.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 * 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.075;
-
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 de 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");
- 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
- }
- }
+ state = THUISPOSITIE_RECHTS; //Door naar de volgende state
+
}
+
+
break; //Stop met alle acties in deze case
}
@@ -971,8 +974,8 @@
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){
+
+ if(puls_motor_arm1.getPosition() <= -84) {
referentie_arm1 = -84;
}
@@ -1027,8 +1030,8 @@
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){
+
+ if(puls_motor_arm1.getPosition() <= -84) {
referentie_arm1 = -84;
}
@@ -1147,8 +1150,8 @@
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){
+
+ if(puls_motor_arm1.getPosition() <= 48) {
referentie_arm1 = 48;
}
@@ -1219,8 +1222,8 @@
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){
+
+ if(puls_motor_arm1.getPosition() <= 48) {
referentie_arm1 = 48;
}
@@ -1275,8 +1278,8 @@
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){
+
+ if(puls_motor_arm1.getPosition() <= 48) {
referentie_arm1 = 48;
}