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:
- 10:52b22bff409a
- Parent:
- 9:454e7da8ab65
- Child:
- 11:e9b906b5f572
--- a/main.cpp Mon Oct 20 20:06:06 2014 +0000
+++ b/main.cpp Tue Oct 21 11:57:27 2014 +0000
@@ -40,59 +40,61 @@
int i; //
volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
-void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
- regelaar_ticker_flag = true;
+void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
+{
+ regelaar_ticker_flag = true;
}
volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
-void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
- regelaar_EMG_flag = true;
+void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
+{
+ regelaar_EMG_flag = true;
}
-void keep_in_range(float * in, float min, float max){ //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
- if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum
+void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
+{
+ if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
*in = min;
}
- if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum
+ if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
*in = max;
- }
- else { //In alle andere gevallen is de waarde de waarde zelf
+ } else { //In alle andere gevallen is de waarde de waarde zelf
*in = *in;
}
}
-void arm1_naar_thuispositie(){
+void arm1_naar_thuispositie()
+{
pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
-
- if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1
- dir_motor_arm1.write(1);
- }
- else { //Anders richting nul
+
+ if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
+ dir_motor_arm1.write(1);
+ } else { //Anders richting nul
dir_motor_arm1.write(0);
}
pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
- pc.printf("pulsmotorgetposition %f\n\r", puls_motor_arm1.getPosition());
+ pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
-
+
if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie
- }
-}
+ }
+}
-void arm2_naar_thuispositie(){
+void arm2_naar_thuispositie()
+{
pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
-
- if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1
+
+ if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
dir_motor_arm2.write(1);
- }
- else { //Anders richting nul
+ } else { //Anders richting nul
dir_motor_arm2.write(0);
}
pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
-
+
if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie
@@ -100,84 +102,80 @@
}
-int main() {
-
- //PC baud rate instellen
- pc.baud(38400); //PC baud rate is 38400 bits/seconde
-
- switch(state) //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
- {
-
- case RUST: //Aanzetten
- {
- lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
- lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
- wait(2); //Twee seconden wachten
- pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten
- state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
- break; //Stopt acties in deze case
- }
-
- case KALIBRATIE_ARM1: //Arm 1 naar thuispositie
- {
- wait(1); //Een seconde wachten
-
- ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
-
- while(state == KALIBRATIE_ARM1){
- 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
-
- arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
+int main()
+{
+ while(1) {
+ //PC baud rate instellen
+ pc.baud(38400); //PC baud rate is 38400 bits/seconde
+
+ switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
+
+ case RUST: { //Aanzetten
+ lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
+ lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
+ wait(2); //Twee seconden wachten
+ pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten
+ state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
+ break; //Stopt acties in deze case
+ }
+
+ case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
+ pc.printf("kalibratie-arm1");
+ wait(1); //Een seconde wachten
+ ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
+
+ while(state == KALIBRATIE_ARM1) {
+ 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
+
+ arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
+ }
+ wait(1); //Een seconde wachten
+ break; //Stopt acties in deze case
+ }
+
+ case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
+ wait(1); //Een seconde wachten
+
+ //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
+
+ while(state == KALIBRATIE_ARM2) {
+ 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
+
+ arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
+ }
+ wait(1); //Een seconde wachten
+ ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
+ break; //Stopt acties in deze case
+ }
+
+ case KALIBRATIE_BICEPS: { //Kalibratie EMG signaal biceps
+ wait(1); //Een seconde wachten
+
+ ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
+
+ pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
+
+ //5 seconden EMG biceps meten
+
+ wait(1); //Een seconde wachten
+ lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
+ lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
+
+ for (i=0; i<1000; i++) {
+ 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
+
+ xbk = EMG_bi.read(); //EMG signaal uitlezen
+ }
+ break;
+
+ }
+ default: {
+ state = RUST;
+ }
}
- wait(1); //Een seconde wachten
- break; //Stopt acties in deze case
+ pc.printf("state: %u\n",state);
}
-
- case KALIBRATIE_ARM2: //Arm 2 naar thuispositie
- {
- wait(1); //Een seconde wachten
-
- //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
-
- while(state == KALIBRATIE_ARM2){
- 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
-
- arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
- }
- wait(1); //Een seconde wachten
- ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
- break; //Stopt acties in deze case
- }
-
- case KALIBRATIE_BICEPS: //Kalibratie EMG signaal biceps
- {
- wait(1); //Een seconde wachten
-
- ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
-
- pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie
-
- //5 seconden EMG biceps meten
-
- wait(1); //Een seconde wachten
- lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
- lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
-
- for (i=0; i<1000; i++){
- 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
-
- xbk = EMG_bi.read(); //EMG signaal uitlezen
- }
-
- }
-
-
- }
-
-
-
-
}
\ No newline at end of file