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:
- 7:dd3cba61b34b
- Parent:
- 6:3b6fad529520
- Child:
- 8:d4161e9be1da
- Child:
- 9:454e7da8ab65
--- a/main.cpp Thu Oct 16 15:09:46 2014 +0000
+++ b/main.cpp Fri Oct 17 07:52:22 2014 +0000
@@ -34,6 +34,13 @@
char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
+int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
+int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
+float pwm_to_motor_arm1; //PWM output naar motor arm 1
+float pwm_to_motor_arm2; //PWM output naar motor arm 2
+float xbk; //Gemeten EMG waarde biceps in de kalibratie
+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;
@@ -55,14 +62,42 @@
*in = *in;
}
}
-
-int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is
-int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is
-float pwm_to_motor_arm1; //PWM output naar motor arm 1
-float pwm_to_motor_arm2; //PWM output naar motor arm 2
-float xbk; //Gemeten EMG waarde biceps in de kalibratie
-int i = 0; //
+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
+ dir_motor_arm1.write(0);
+ }
+ pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
+
+ 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)
+ kalibratie_positie_arm1 = true;
+ pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 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
+ dir_motor_arm2.write(1);
+ }
+ else { //Anders richting nul
+ dir_motor_arm2.write(0);
+ }
+ pwm_motor_arm2.write(abs(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)
+ kalibratie_positie_arm2 = true;
+ pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
+ }
+}
int main() {
@@ -91,28 +126,13 @@
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
- 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
- dir_motor_arm1.write(0);
- }
- pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
-
- 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)
- kalibratie_positie_arm1 = true;
- pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie
- }
- wait(1); //Een seconde wachten
-
+ arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
}
+ wait(1); //Een seconde wachten
}
//Arm 2 naar thuispositie
- if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
+ if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
wait(1); //Een seconde wachten
//ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
@@ -121,21 +141,7 @@
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
- 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
- dir_motor_arm2.write(1);
- }
- else { //Anders richting nul
- dir_motor_arm2.write(0);
- }
- pwm_motor_arm2.write(abs(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)
- kalibratie_positie_arm2 = true;
- pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
- }
+ 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
@@ -154,8 +160,8 @@
lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
wait(2); //Twee seconden wachten
- for (i=0 ,i<5000, i++){
- while(setregelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ 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