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:
- 6:3b6fad529520
- Parent:
- 5:a1dcd605dd3d
- Child:
- 7:dd3cba61b34b
--- a/main.cpp Thu Oct 16 13:32:57 2014 +0000
+++ b/main.cpp Thu Oct 16 15:09:46 2014 +0000
@@ -4,9 +4,9 @@
#include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
//Constanten definiëren en waarde geven
-#define SAMPLETIME_REGELAAR 0.005
-#define KP 1
-#define SAMPLETIME_EMG 0.005
+#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
+#define KP 1 //Factor proprotionele regelaar
+#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
//Pinverdeling en naamgeving variabelen
TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
@@ -15,56 +15,54 @@
PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1
-PwmOut pwm_motor_arm2(PTA12);
-DigitalOut dir_motor_arm2(PTD3);
-Encoder puls_motor_arm2(PTD4, PTC8);
-AnalogIn EMG_bi(PTB1);
+PwmOut pwm_motor_arm2(PTA12); //PWM naar motor arm 2
+DigitalOut dir_motor_arm2(PTD3); //Ricting van motor arm 2
+Encoder puls_motor_arm2(PTD4, PTC8); //Encoder pulsen tellen van motor arm 2
+AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
-Ticker ticker_regelaar;
-Ticker ticker_EMG;
+Ticker ticker_regelaar; //Ticker voor regelaar motor
+Ticker ticker_EMG; //Ticker voor EMG meten
-//Gedefinieerde datatypen en naamgeving
+//Gedefinieerde datatypen en naamgeving en beginwaarden
bool rust = false; //Bool voor controleren volgorde in programma
bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma
-bool kalibratie_positie_arm2 = false;
-bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma
-bool kalibratie_EMG_tri = false;
+bool kalibratie_positie_arm2 = false; //Bool voor controleren volgorde in programma
+bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma
+bool kalibratie_EMG_tri = false; //Bool voor controleren volgorde in programma
-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
+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
-volatile bool regelaar_ticker_flag;
-void setregelaar_ticker_flag(){
- regelaar_ticker_flag = true;
+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;
}
-volatile bool regelaar_EMG_flag;
-void setregelaar_EMG_flag(){
- regelaar_EMG_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 keep_in_range(float * in, float min, float max){
- if (*in < min){
+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){
+ if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum
*in = max;
}
- else {
+ else { //In alle andere gevallen is de waarde de waarde zelf
*in = *in;
}
}
-int puls_arm1_home = 363;
-int puls_arm2_home = 787;
-float pwm_to_motor_arm1;
-float pwm_to_motor_arm2;
-float xbk;
-int i = 0;
-
-//Beginwaarden voor variabelen
+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; //
int main() {
@@ -90,25 +88,25 @@
ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
- while(regelaar_ticker_flag != true) ;
- regelaar_ticker_flag = false;
+ 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;
- keep_in_range(&pwm_to_motor_arm1, -1, 1);
+ 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){
+ if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1
dir_motor_arm1.write(1);
}
- else {
+ else { //Anders richting nul
dir_motor_arm1.write(0);
}
- pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
+ 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) {
+ 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");
+ pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie
}
- wait(1);
+ wait(1); //Een seconde wachten
}
}
@@ -120,46 +118,47 @@
//ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) {
- while(regelaar_ticker_flag != true) ;
- regelaar_ticker_flag = false;
+ 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;
- keep_in_range(&pwm_to_motor_arm2, -1, 1);
+ 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){
+ if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1
dir_motor_arm2.write(1);
}
- else {
+ else { //Anders richting nul
dir_motor_arm2.write(0);
}
- pwm_motor_arm2.write(abs(pwm_to_motor_arm2));
+ 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) {
+ 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");
+ pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie
}
}
- wait(1);
- ticker_regelaar.detach();
+ wait(1); //Een seconde wachten
+ ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
}
//Ticker voor kalibratie
if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
- ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
- pc.printf("Ticker voor kalibratie compleet");
+ 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);
- lcd_r1 = " EMG kalibratie ";
- lcd_r2 = " Span biceps aan";
+ 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
+ wait(2); //Twee seconden wachten
for (i=0 ,i<5000, i++){
- while(setregelaar_EMG_flag != true) ;
- regelaar_EMG_flag = false;
+ while(setregelaar_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();
+ xbk = EMG_bi.read(); //EMG signaal uitlezen
}
}