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:
- 19:38c9d177b6ee
- Parent:
- 18:d3a7f8b4cb22
- Child:
- 20:1cb0cf0d49ac
--- a/main.cpp Thu Oct 30 11:35:04 2014 +0000
+++ b/main.cpp Thu Oct 30 16:12:46 2014 +0000
@@ -6,15 +6,13 @@
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP_arm1 //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 4 //Factor proprotionele regelaar arm 2
-#define KI_arm2 0.5 //Factor integraal regelaar arm 2
-#define KD_arm2 0.5 //Factor afgeleide regelaar arm 2
-#define SAMPLETIME_EMG 0 //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
+#define KP_arm1 0.5 //Factor proprotionele regelaar arm 1
+#define KI_arm1 0.1 //Factor integraal regelaar arm 1
+#define KD_arm1 0.01 //Factor afgeleide regelaar arm 1
+#define KP_arm2 0.3 //Factor proprotionele regelaar arm 2
+#define KI_arm2 0.1 //Factor integraal regelaar arm 2
+#define KD_arm2 0.1 //Factor afgeleide regelaar arm 2
+#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
//High Pass filter
@@ -79,7 +77,8 @@
float xb; //Gemeten EMG waarde biceps
float xt; //Gemeten EMG waarde triceps
-float puls_arm1_home = 0;
+float referentie_arm1 = 0;
+float referentie_arm2 = 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
@@ -128,72 +127,64 @@
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
- {
+ 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() //Brengt arm 1 naar de beginpositie
{
- puls_arm1_home = puls_arm1_home + 180/200;
- error_arm1_kalibratie = (puls_arm1_home - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
- integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
- afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
- pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
- keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ referentie_arm1 = referentie_arm1 + 180.0/200.0;
+
+ if (referentie_arm1 >= 180) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
+ referentie_arm1 = 180;
- if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1
- {
+ }
+ error_arm1_kalibratie = (referentie_arm1 - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
+ 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);
- }
- else //Anders richting nul
- {
+ } else { //Anders richting nul
dir_motor_arm1.write(0);
}
- pwm_to_motor_arm1=pwm_to_motor_arm1/20;
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()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
- if (error_arm1_kalibratie == 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\n"); //Tekst voor controle Arm 1 naar thuispositie
- }
+ state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
+ pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
+
}
void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
{
- error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ referentie_arm2 = referentie_arm2 + 35/200;
+ error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
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
pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); //Huidige PWM waarde naar pc sturen
- 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)
- {
+ 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 = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
}
@@ -202,13 +193,13 @@
void filter_biceps() //Filtert het EMG signaal van de biceps
{
arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); //Highpass filter voor het EMG signaal van de biceps, filtert de xb en schrijft het over in de xbf, 1 EMG waarde filteren per keer
-
+
arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1); //Notch filter voor het EMG signaal van de biceps, filtert de xbf uit de highpass en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
xbf = fabs(xbf); //Rectifier, schrijft de absolute waarde van de xbf uit de notch opnieuw in de xbf, fabs omdat het een float is
arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); //Lowpass filter voor het EMG signaal van de biceps, filtert de xbf uit de rectifier en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
-
+
pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen
}
@@ -228,16 +219,14 @@
int main() //Main script
{
- while(1) //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
- {
+ while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
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
- {
+ 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
- {
- pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
+ case RUST: { //Aanzetten
+ pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm
lcd.locate(0,1); //Zet de tekst op de tweede regel
@@ -249,15 +238,13 @@
break; //Stopt acties in deze case
}
- case KALIBRATIE_ARM1: //Arm 1 naar thuispositie
- {
+ case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen
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(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
@@ -267,15 +254,13 @@
break; //Stopt acties in deze case
}
- case KALIBRATIE_ARM2: //Arm 2 naar thuispositie
- {
- pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
+ case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
+ pc.printf("KALIBRATIE_ARM2\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
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(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
@@ -286,10 +271,9 @@
break; //Stopt acties in deze case
}
- case EMG_KALIBRATIE_BICEPS: //Kalibratie biceps voor bepalen threshold
- {
+ case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold
pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
-
+
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
lcd.locate(0,1); //Zet tekst op tweede regel
@@ -302,9 +286,8 @@
ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
- while(biceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten
- {
-
+ while(biceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
+
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
@@ -312,51 +295,44 @@
filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren
- if(int(biceps_kalibratie.read()) == 0) //Wanneer de timer nog geen een seconde heeft gemeten
- {
+ if(int(biceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen een seconde heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 5 "); //Tekst op LCD scherm
pc.printf("4"); //Controle naar pc sturen
}
- if(int(biceps_kalibratie.read()) == 1) //Wanneer de timer nog geen twee seconden heeft gemeten
- {
+ if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 4 "); //Tekst op LCD scherm
pc.printf("3"); //Controle naar pc sturen
}
- if(int(biceps_kalibratie.read()) == 2) //Wanneer de timer nog geen drie seconden heeft gemeten
- {
+ if(int(biceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 3 "); //Tekst op LCD scherm
pc.printf("2"); //Controle naar pc sturen
}
- if(int(biceps_kalibratie.read()) == 3) //Wanneer de timer nog geen vier seconden heeft gemeten
- {
+ if(int(biceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 2 "); //Tekst op LCD scherm
pc.printf("1"); //Controle naar pc sturen
}
- if(int(biceps_kalibratie.read()) == 4) //Wanneer de timer nog geen vijf seconden heeft gemeten
- {
+ if(int(biceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 1 "); //Tekst op LCD scherm
pc.printf("1"); //Controle naar pc sturen
}
}
- if(xbf >= xbfmax) //Als de gefilterde EMG waarde groter is dan xbfmax
- {
+ if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
}
-
+
xbt = xbfmax * 0.8; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen
state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps
break; //Stopt alle acties in deze case
}
- case EMG_KALIBRATIE_TRICEPS:
- {
+ case EMG_KALIBRATIE_TRICEPS: {
pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen
lcd.locate(0,0); //Zet de tekst op de eerste regel
@@ -364,15 +340,14 @@
lcd.locate(0,1); //Zet tekst op eerste regel
lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm
- arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states); //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
+ arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states); //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
arm_biquad_cascade_df1_init_f32(¬ch_triceps, 2, notch_triceps_const, notch_triceps_states); //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren
arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 2, lowpass_triceps_const, lowpass_triceps_states); //Lowpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
//ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Deze ticker loopt nog
triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
- while(triceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten
- {
+ while(triceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
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
@@ -380,43 +355,37 @@
filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten
- if(int(triceps_kalibratie.read()) == 0) //Wanneer de timer nog geen twee seconden heeft gemeten
- {
+ if(int(triceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen twee seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 5 "); //Tekst op LCD scherm
pc.printf("4"); //Controle naar pc sturen
}
- if(int(triceps_kalibratie.read()) == 1) //Wanneer de timer nog geen twee seconden heeft gemeten
- {
+ if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 4 "); //Tekst op LCD scherm
pc.printf("3"); //Controle naar pc sturen
}
- if(int(triceps_kalibratie.read()) == 2) //Wanneer de timer nog geen drie seconden heeft gemeten
- {
+ if(int(triceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 3 "); //Tekst op LCD scherm
pc.printf("2"); //Controle naar pc sturen
}
- if(int(triceps_kalibratie.read()) == 3) //Wanneer de timer nog geen vier seconden heeft gemeten
- {
+ if(int(triceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 2 "); //Tekst op LCD scherm
pc.printf("1"); //Controle naar pc sturen
}
- if(int(triceps_kalibratie.read()) == 4) //Wanneer de timer nog geen vijf seconden heeft gemeten
- {
+ if(int(triceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
lcd.locate(0,0); //Zet de tekst op de eerste regel
lcd.printf(" 1 "); //Tekst op LCD scherm
pc.printf("1"); //Controle naar pc sturen
}
}
- if(xtf >= xtfmax) //Als de gefilterde EMG waarde groter is dan xtfmax
- {
+ if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
}
-
+
xtt = xtfmax * 0.8; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen
state = START; //Gaat door naar het slaan, kalibratie nu afgerond
@@ -426,11 +395,10 @@
case START: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" START "); //Tekst op LCD scherm
-
+
pc.printf("START\n\r"); //Controle naar pc sturen
-
- while(state == START)
- {
+
+ while(state == START) {
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
@@ -439,26 +407,46 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = B; //Ga door naar state B
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = T; //Ga door naar state T
}
}
break; //Stopt met de acties in deze case
}
-
- case B:
- {
+
+ case B: {
lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" B "); //Tekst op LCD scherm
+ lcd.printf(" B "); //Tekst op LCD scherm
pc.printf("B\n\r"); //Controle naar pc sturen
- while(state == B)
- {
+ 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
+
+ xb = EMG_bi.read(); //EMG signaal biceps uitlezen
+ filter_biceps(); //EMG signaal biceps filteren
+ xt = EMG_tri.read(); //EMG signaal triceps uitlezen
+ filter_triceps(); //EMG signaal triceps filteren
+
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BB; //Ga door naar state BB
+ }
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BT; //Ga door naar state BT
+ }
+ }
+ break; //Stop met alle acties in deze case
+ }
+
+ case T: {
+ 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
+
+ while(state == T) {
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
@@ -467,26 +455,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BB; //Ga door naar state BB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = TB; //Ga door naar state TB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BT; //Ga door naar state BT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = TT; //Ga door naar state TT
}
}
break; //Stop met alle acties in deze case
}
- case T:
- {
+ case BB: {
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
+ lcd.printf(" BB "); //Tekst op LCD scherm
+ pc.printf("BB\n\r"); //Controle naar pc sturen
- while(state == T)
- {
+ while(state == BB) {
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
@@ -495,26 +479,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = TB; //Ga door naar state TB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BBB; //Ga door naar state BBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = TT; //Ga door naar state TT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BBT; //Ga door naar state BBT
}
}
break; //Stop met alle acties in deze case
}
-
- case BB:
- {
+
+ case BT: {
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
+ lcd.printf(" BT "); //Tekst op LCD scherm
+ pc.printf("BT\n\r"); //Controle naar pc sturen
- while(state == BB)
- {
+ while(state == BT) {
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
@@ -523,26 +503,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BBB; //Ga door naar state BBB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BTB; //Ga door naar state BTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BBT; //Ga door naar state BBT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BTT; //Ga door naar state BTT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case BT:
- {
+
+ case TB: {
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
+ lcd.printf(" TB "); //Tekst op LCD scherm
+ pc.printf("TB\n\r"); //Controle naar pc sturen
- while(state == BT)
- {
+ while(state == TB) {
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
@@ -551,26 +527,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BTB; //Ga door naar state BTB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = TBB; //Ga door naar state TBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BTT; //Ga door naar state BTT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = TBT; //Ga door naar state TBT
}
}
- break; //Stop met alle acties in deze case
- }
+ break; //Stop met alle acties in deze case
+ }
- case TB:
- {
+ case TT: {
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
+ lcd.printf(" TT "); //Tekst op LCD scherm
+ pc.printf("TT\n\r"); //Controle naar pc sturen
- while(state == TB)
- {
+ while(state == TT) {
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
@@ -579,26 +551,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = TBB; //Ga door naar state TBB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = TTB; //Ga door naar state TTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = TBT; //Ga door naar state TBT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = TTT; //Ga door naar state TTT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case TT:
- {
+
+ case BBB: {
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
+ lcd.printf(" BBB "); //Tekst op LCD scherm
+ pc.printf("BBB\n\r"); //Controle naar pc sturen
- while(state == TT)
- {
+ while(state == BBB) {
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
@@ -607,26 +575,23 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = TTB; //Ga door naar state TTB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BBBB; //Ga door naar state BBBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = TTT; //Ga door naar state TTT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BBBT; //Ga door naar state BBBT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
- case BBB:
- {
+
+ case BBT: {
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
+ lcd.printf(" BBT "); //Tekst op LCD scherm
+ pc.printf("BBT\n\r"); //Controle naar pc sturen
- while(state == BBB)
- {
+ while(state == BBT) {
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
@@ -635,27 +600,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BBBB; //Ga door naar state BBBB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BBTB; //Ga door naar state BBTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BBBT; //Ga door naar state BBBT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BBTT; //Ga door naar state BBTT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
-
- case BBT:
- {
+
+ case BTB: {
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
+ lcd.printf(" BTB "); //Tekst op LCD scherm
+ pc.printf("BTB\n\r"); //Controle naar pc sturen
- while(state == BBT)
- {
+ while(state == BTB) {
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
@@ -664,26 +624,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BBTB; //Ga door naar state BBTB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BTBB; //Ga door naar state BTBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BBTT; //Ga door naar state BBTT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BTBT; //Ga door naar state BTBT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case BTB:
- {
+
+ case BTT: {
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
+ lcd.printf(" BTT "); //Tekst op LCD scherm
+ pc.printf("BTT\n\r"); //Controle naar pc sturen
- while(state == BTB)
- {
+ while(state == BTT) {
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
@@ -692,26 +648,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BTBB; //Ga door naar state BTBB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = BTTB; //Ga door naar state BTTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BTBT; //Ga door naar state BTBT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = BTTT; //Ga door naar state BTTT
}
}
- break; //Stop met alle acties in deze case
- }
-
- case BTT:
- {
+ break; //Stop met alle acties in deze case
+ }
+
+ case TBB: {
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
+ lcd.printf(" TBB "); //Tekst op LCD scherm
+ pc.printf("TBB\n\r"); //Controle naar pc sturen
- while(state == BTT)
- {
+ while(state == TBB) {
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
@@ -720,26 +672,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = BTTB; //Ga door naar state BTTB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = TBBB; //Ga door naar state TBBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = BTTT; //Ga door naar state BTTT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = TBBT; //Ga door naar state TBBT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case TBB:
- {
+
+ case TBT: {
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
+ lcd.printf(" TBT "); //Tekst op LCD scherm
+ pc.printf("TBT\n\r"); //Controle naar pc sturen
- while(state == TBB)
- {
+ while(state == TBT) {
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
@@ -748,26 +696,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = TBBB; //Ga door naar state TBBB
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ state = TBTB; //Ga door naar state TBTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = TBBT; //Ga door naar state TBBT
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ state = TBTT; //Ga door naar state TBTT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case TBT:
- {
+
+ case TTB: {
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
+ lcd.printf(" BBB "); //Tekst op LCD scherm
+ pc.printf("BBB\n\r"); //Controle naar pc sturen
- while(state == TBT)
- {
+ while(state == BBB) {
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
@@ -776,54 +720,22 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
- state = TBTB; //Ga door naar state TBTB
- }
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
- state = TBTT; //Ga door naar state TBTT
- }
- }
- break; //Stop met alle acties in deze case
- }
-
- case TTB:
- {
- 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
-
- while(state == BBB)
- {
- 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
-
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
-
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTBB; //Ga door naar state TTBB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TTBT; //Ga door naar state TTBT
}
}
- break; //Stop met alle acties in deze case
- }
-
- case TTT:
- {
+ break; //Stop met alle acties in deze case
+ }
+
+ case TTT: {
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
- while(state == TTT)
- {
+ while(state == TTT) {
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
@@ -832,232 +744,238 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
- {
+ if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTTB; //Ga door naar state TTTB
}
- if(xt >= xtt) //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
- {
+ if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TTTT; //Ga door naar state TTTT
}
}
- break; //Stop met alle acties in deze case
+ break; //Stop met alle acties in deze case
}
-
- case BBBB:
- {
+
+ case BBBB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBBB "); //Tekst op LCD scherm
pc.printf("BBBB\n\r"); //Controle naar pc sturen
- while(state == BBBB)
- {
+ while(state == BBBB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
- }
-
- case BBBT:
- {
+ }
+ break; //Stop met alle acties in deze case
+ }
+
+ case BBBT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBBT "); //Tekst op LCD scherm
pc.printf("BBBT\n\r"); //Controle naar pc sturen
- while(state == BBBT)
- {
+ while(state == BBBT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BBTB:
- {
+
+ case BBTB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBTB "); //Tekst op LCD scherm
pc.printf("BBTB\n\r"); //Controle naar pc sturen
- while(state == BBTB)
- {
+ while(state == BBTB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BBTT:
- {
+
+ case BBTT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBTT "); //Tekst op LCD scherm
pc.printf("BBTT\n\r"); //Controle naar pc sturen
- while(state == BBTT)
- {
+ while(state == BBTT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BTBB:
- {
+
+ case BTBB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTBB "); //Tekst op LCD scherm
pc.printf("BTBB\n\r"); //Controle naar pc sturen
- while(state == BTBB)
- {
+ while(state == BTBB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BTBT:
- {
+
+ case BTBT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTBT "); //Tekst op LCD scherm
pc.printf("BTBT\n\r"); //Controle naar pc sturen
- while(state == BTBT)
- {
+ while(state == BTBT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BTTB:
- {
+
+ case BTTB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTTB "); //Tekst op LCD scherm
pc.printf("BTTB\n\r"); //Controle naar pc sturen
- while(state == BTTB)
- {
+ while(state == BTTB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case BTTT:
- {
+
+ case BTTT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTTT "); //Tekst op LCD scherm
pc.printf("BTTT\n\r"); //Controle naar pc sturen
- while(state == BTTT)
- {
+ while(state == BTTT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TBBB:
- {
+
+ case TBBB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBBB "); //Tekst op LCD scherm
pc.printf("TBBB\n\r"); //Controle naar pc sturen
- while(state == TBBB)
- {
+ while(state == TBBB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TBBT:
- {
+
+ case TBBT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBBT "); //Tekst op LCD scherm
pc.printf("TBBT\n\r"); //Controle naar pc sturen
- while(state == TBBT)
- {
+ while(state == TBBT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TBTB:
- {
+
+ case TBTB: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBBB "); //Tekst op LCD scherm
pc.printf("TBBB\n\r"); //Controle naar pc sturen
- while(state == TBBB)
- {
+ while(state == TBBB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TBTT:
- {
+
+ case TBTT: {
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)
- {
+ while(state == TBTT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TTBB:
- {
+
+ 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)
- {
+ while(state == TTBB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TTBT:
- {
+
+ 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)
- {
+ while(state == TTBT) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TTTB:
- {
+
+ 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)
- {
+ while(state == TTTB) {
//Motoractie
- }
- break; //Stop met alle acties in deze case
+ }
+ break; //Stop met alle acties in deze case
}
-
- case TTTT:
- {
+
+ case TTTT: {
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 == TTBB)
- {
- //Motoractie
- }
- break; //Stop met alle acties in deze case
+ 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
+
+ float referentie_speed_TTTT;
+ float error_speed_TTTT;
+ float integraal_speed_TTTT = 0;
+ float afgeleide_speed_TTTT;
+ float vorige_error_speed_TTTT = 0;
+ float pwm_to_motor_speed_TTTT;
+#define KPs 0.001
+#define KIs 0
+#define KDs 0
+
+ referentie_speed_TTTT = referentie_speed_TTTT + 1902/40;
+ error_speed_TTTT = (referentie_speed_TTTT - puls_motor_arm2.getSpeed());
+ integraal_speed_TTTT = integraal_speed_TTTT + error_speed_TTTT*SAMPLETIME_REGELAAR;
+ afgeleide_speed_TTTT = (error_speed_TTTT - vorige_error_speed_TTTT)/SAMPLETIME_REGELAAR;
+ pwm_to_motor_speed_TTTT = error_speed_TTTT*KPs + integraal_speed_TTTT*KIs + afgeleide_speed_TTTT*KDs;
+ keep_in_range(&pwm_to_motor_speed_TTTT, -1, 1);
+
+ if(pwm_to_motor_speed_TTTT > 0) {
+ dir_motor_arm2.write(1);
+ } else {
+ dir_motor_arm2.write(0);
+ }
+
+ pwm_motor_arm1.write(0);
+
+ if(puls_motor_arm2.getPosition() >= 97) {
+ pwm_motor_arm1.write(-fabs(pwm_to_motor_speed_TTTT));
+ }
+
+ if(referentie_speed_TTTT == 1902) {
+ referentie_speed_TTTT = 1902;
+ }
+
+ referentie_arm1 = referentie_arm1 + 180/200;
+
+ break; //Stop met alle acties in deze case
+ }
+
+ 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);
}
-
- 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)
}
- }
-}
\ No newline at end of file
+ }
\ No newline at end of file