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:
- 23:5267c928ae2b
- Parent:
- 22:838a17065bc7
- Child:
- 24:a1fdc830f96c
--- a/main.cpp Fri Oct 31 14:08:57 2014 +0000
+++ b/main.cpp Sun Nov 02 17:08:31 2014 +0000
@@ -9,7 +9,7 @@
#define KP_arm1 0.02 //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.05 //Factor proprotionele regelaar arm 2
+#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 SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
@@ -46,7 +46,7 @@
PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
-Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
+Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1, (geel, wit)
PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
@@ -63,7 +63,7 @@
Timer EMG;
//States definiëren
-enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
+enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT, WACHT, THUISPOSITIE_MIDDEN}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
uint8_t state = RUST; //State is rust aan het begin
//Gedefinieerde datatypen en naamgeving en beginwaarden
@@ -83,6 +83,8 @@
float referentie_arm1 = 0;
float referentie_arm2 = 0;
+float t = 0;
+
arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1
float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
@@ -150,9 +152,9 @@
keep_in_range(&pwm_to_motor_arm1, -1, 1);
if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
- dir_motor_arm1.write(1);
+ dir_motor_arm1.write(0);
} else { //Anders richting nul
- dir_motor_arm1.write(0);
+ dir_motor_arm1.write(1);
}
pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
@@ -270,20 +272,20 @@
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_arm2 = referentie_arm2 + 35.0/200.0;
+ referentie_arm2 = referentie_arm2 + 123.0/200.0;
pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen
pc.printf("referentie2 %f\n\r", referentie_arm2);
- if(referentie_arm2 >= 35) {
- referentie_arm2 = 35;
+ if(referentie_arm2 >= 123) {
+ referentie_arm2 = 123;
state = EMG_KALIBRATIE_BICEPS;
}
}
wait(1); //Een seconde wachten
- ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
+ //ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
break; //Stopt acties in deze case
}
@@ -444,23 +446,22 @@
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
-
+
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
-
+
while(state == B) {
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();
-
+
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BB; //Ga door naar state BB
}
@@ -475,14 +476,13 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" T "); //Tekst op LCD scherm
pc.printf("T\n\r"); //Controle naar pc sturen
-
+
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -506,15 +506,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BB "); //Tekst op LCD scherm
pc.printf("BB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -538,15 +537,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BT "); //Tekst op LCD scherm
pc.printf("BT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -570,15 +568,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TB "); //Tekst op LCD scherm
pc.printf("TB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -602,15 +599,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TT "); //Tekst op LCD scherm
pc.printf("TT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -634,15 +630,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBB "); //Tekst op LCD scherm
pc.printf("BBB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -667,15 +662,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBT "); //Tekst op LCD scherm
pc.printf("BBT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -699,15 +693,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTB "); //Tekst op LCD scherm
pc.printf("BTB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -731,15 +724,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTT "); //Tekst op LCD scherm
pc.printf("BTT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -763,15 +755,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBB "); //Tekst op LCD scherm
pc.printf("TBB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -795,15 +786,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBT "); //Tekst op LCD scherm
pc.printf("TBT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -827,15 +817,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TTB "); //Tekst op LCD scherm
pc.printf("TTB\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -859,15 +848,14 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TTT "); //Tekst op LCD scherm
pc.printf("TTT\n\r"); //Controle naar pc sturen
-
+
EMG.reset();
EMG.start();
-
- while(EMG.read() <= 3)
- {
+
+ while(EMG.read() <= 3) {
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();
}
@@ -1008,83 +996,248 @@
break; //Stop met alle acties in deze case
}
- case TBTT: {
+ case TBTT: { //Motoraansturing voor richten op doel middenboven
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBTT "); //Tekst op LCD scherm
pc.printf("TBTT\n\r"); //Controle naar pc sturen
while(state == TBTT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTBB: {
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTBB "); //Tekst op LCD scherm
- pc.printf("TTBB\n\r"); //Controle naar pc sturen
-
- while(state == TTBB) {
- //Motoractie
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTBT: {
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTBT "); //Tekst op LCD scherm
- pc.printf("TTBT\n\r"); //Controle naar pc sturen
-
- while(state == TTBT) {
- //Motoractie
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTTB: {
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTTB "); //Tekst op LCD scherm
- pc.printf("TTTB\n\r"); //Controle naar pc sturen
-
- while(state == TTTB) {
- //Motoractie
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTTT: {
- lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TTTT "); //Tekst op LCD scherm
- pc.printf("TTTT\n\r"); //Controle naar pc sturen
-
- while(state == TTTT) {
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
//Positie arm 1 is goed
//Snelheid arm 2
+ while(puls_motor_arm1.getPosition() > 48) {
+ referentie_arm1 = referentie_arm1 - 132.0/200.0;
+ pc.printf("referentie arm 1 %f ", referentie_arm1);
+ pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
+ pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
+ }
+ pc.printf("arm 1 op positie");
- referentie_arm2 = referentie_arm2 + 0.1902;
+ while(puls_motor_arm2.getPosition() < 167) {
+ referentie_arm2 = referentie_arm2 + 44.0/200.0;
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
+ }
+ pc.printf("arm2 op positie");
+
+
+ if(referentie_arm2 >= 167) {
+ while(puls_motor_arm2.getPosition() > -8) {
+
+
+
+ referentie_arm2 = -0.5 * 200000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= -8) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > -100) {
+ referentie_arm2 = referentie_arm2 + 167.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+
+
+
+ if(puls_motor_arm2.getPosition() >= 167) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
+ }
+ }
+ }
+ break; //Stop met alle acties in deze case
+ }
+
+ case TTBB: { //Motoraansturing voor richten op doel linksonder
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTBB "); //Tekst op LCD scherm
+ pc.printf("TTBB\n\r"); //Controle naar pc sturen
+
+ while(state == TTBB) {
+ //Motoractie
+ 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
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 200000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
+ }
+ }
+ break; //Stop met alle acties in deze case
+ }
- if(referentie_arm2 >= 9.51) {
- while(pwm_to_motor_arm2 != 0) {
- referentie_arm2 = referentie_arm2 - 90/200;
+ case TTBT: { //Motoraansturing voor terug naar kalibratiepositie
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTBT "); //Tekst op LCD scherm
+ pc.printf("TTBT\n\r"); //Controle naar pc sturen
+
+ while(state == TTBT) {
+ //Motoractie
+ lcd.printf(" GEEN DOEL ");
+ wait(1);
+ state = WACHT;
+ }
+ break; //Stop met alle acties in deze case
+ }
+
+ case TTTB: { //Motoraansturing voor richten op doel linksmidden
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTTB "); //Tekst op LCD scherm
+ pc.printf("TTTB\n\r"); //Controle naar pc sturen
+
+ while(state == TTTB) {
+ //Motoractie
+
+ 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
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 400000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
}
- if(pwm_to_motor_arm2 == 0) {
- state = START;
+ }
+
+ break; //Stop met alle acties in deze case
+ }
+
+
+ case TTTT: { //Motoraansturing voor richten op doel linksboven
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" TTTT "); //Tekst op LCD scherm
+ pc.printf("TTBB\n\r"); //Controle naar pc sturen
+
+ while(state == TTTT) {
+ 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
+
+ //Positie arm 1 is goed
+ //Snelheid arm 2
+
+ referentie_arm2 = 0.5 * 600000 * t * t;
+ t = t + 0.005;
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
+ pc.printf("t is %f\n\r", t);
+
+ if(puls_motor_arm2.getPosition() >= 175) {
+ wait(1);
+ while(puls_motor_arm2.getPosition() > 0) {
+ referentie_arm2 = referentie_arm2 - 175.0/200.0;
+
+
+ pc.printf("referentie arm 2 %f ", referentie_arm2);
+ pc.printf("get position %d ", puls_motor_arm2.getPosition());
+ pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
+ }
+
+
+ if(puls_motor_arm2.getPosition() <= 0) {
+ pc.printf("staat stil\n\r");
+ state = WACHT;
+ }
+
}
}
}
- }
- default: {
- //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
- state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
- }
+
+ case THUISPOSITIE_MIDDEN: {
+ while(puls_motor_arm2.getPosition() > 123) {
+ referentie_arm2 = referentie_arm2 - 44.0/200.0;
+ }
+
+ while(puls_motor_arm1.getPosition() < 180) {
+ referentie_arm1 = referentie_arm1 + 132.0/200.0;
+ }
+
+ state = WACHT;
+ break;
+ }
- pc.printf("state: %u\n\r",state);
+ case WACHT: {
+ lcd.printf(" WACHT ");
+ wait(5);
+ state = START;
+
+
+
+
+ default: {
+ //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
+ state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
+ }
+
+ pc.printf("state: %u\n\r",state);
+ }
+ //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ }
}
- //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
}
-}
-
+}
\ No newline at end of file