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:
- 11:e9b906b5f572
- Parent:
- 10:52b22bff409a
- Child:
- 12:996f9f8e3acc
--- a/main.cpp Tue Oct 21 11:57:27 2014 +0000
+++ b/main.cpp Tue Oct 21 14:51:23 2014 +0000
@@ -4,9 +4,11 @@
#include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
//Constanten definiëren en waarde geven
-#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP 0.001 //Factor proprotionele regelaar
-#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
+#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
+#define KP 0.001 //Factor proprotionele regelaar
+#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
+#define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
+#define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
//Pinverdeling en naamgeving variabelen
TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
@@ -15,12 +17,11 @@
PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1
-PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
+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
+Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2
AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
-
Ticker ticker_regelaar; //Ticker voor regelaar motor
Ticker ticker_EMG; //Ticker voor EMG meten
@@ -32,12 +33,10 @@
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; //
+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; //Voor for-loop
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
@@ -65,52 +64,52 @@
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
+ pwm_to_motor_arm1 = (PULS_ARM1_HOME_KALIBRATIE - 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
+ if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
dir_motor_arm1.write(1);
- } else { //Anders richting nul
+ } 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
+ pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
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
+ 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()
{
- 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
+ pwm_to_motor_arm2 = (PULS_ARM2_HOME_KALIBRATIE - 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
+ 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
+ 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
}
}
int main()
{
- while(1) {
+ while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
//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
+ 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
@@ -131,10 +130,10 @@
arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
}
wait(1); //Een seconde wachten
- break; //Stopt acties in deze case
+ break; //Stopt acties in deze case
}
- case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
+ 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
@@ -145,8 +144,8 @@
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
+ wait(1); //Een seconde wachten
+ ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
break; //Stopt acties in deze case
}
@@ -172,8 +171,8 @@
break;
}
- default: {
- state = RUST;
+ 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",state);