Janine Klein Rot
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp
- Committer:
- JKleinRot
- Date:
- 2014-10-29
- Revision:
- 17:c694a0e63a89
- Parent:
- 16:c34c5d9dfe1a
- Child:
- 18:d3a7f8b4cb22
File content as of revision 17:c694a0e63a89:
#include "mbed.h" //Mbed bibliotheek inladen, standaard functies
#include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
#include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
#include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
#include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
#define KP_arm1 0.01 //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 0.01 //Factor proprotionele regelaar arm 2
#define KI_arm2 0 //Factor integraal regelaar arm 2
#define KD_arm2 0 //Factor afgeleide regelaar arm 2
#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
//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
//High Pass filter
#define A1 1
#define A2 -1.5610
#define A3 0.6414
#define B1 0.0201
#define B2 0.0402
#define B3 0.0201
//Notch filter
#define C1 1
#define C2 -1.1873E-16
#define C3 0.9391
#define D1 0.9695
#define D2 -1.1873E-16
#define D3 0.9695
//Low pass filter
#define E1 1
#define E2 -1.9645
#define E3 0.9651
#define F1 1.5515E-4
#define F2 3.1030E-4
#define F3 1.5515E-4
//Pinverdeling en naamgeving variabelen
TextLCD lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2); //LCD scherm
MODSERIAL pc(USBTX, USBRX); //PC
PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
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, (geel, wit)
AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps
AnalogIn EMG_tri(PTB0); //Uitlezen EMG signaal triceps
//Blauw op 3,3 V en groen op GND, voeding beide encoders
Ticker ticker_regelaar; //Ticker voor regelaar motor
Ticker ticker_EMG; //Ticker voor EMG meten
Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
//States definiëren
enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
uint8_t state = RUST; //State is rust aan het begin
//Gedefinieerde datatypen en naamgeving en beginwaarden
float pwm_to_motor_arm1; //PWM output naar motor arm 1
float pwm_to_motor_arm2; //PWM output naar motor arm 2
float error_arm1_kalibratie; //Error in pulsen arm 1
float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
float error_arm2_kalibratie; //Error in pulsen arm 2
float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
float xb; //Gemeten EMG waarde biceps
float xt; //Gemeten EMG waarde triceps
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
float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1
float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
float notch_biceps_states[8]; //Aantal states van het filter
arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1
float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
float lowpass_biceps_states[8]; //Aantal states van het filter
float xbf; //Gefilterde EMG waarde biceps
float xbfmax = 0; //Maximale gefilterde EMG waarde biceps
arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
float highpass_triceps_states[8]; //Aantal states van het filter
arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
float notch_triceps_states[8]; //Aantal states van het filter
arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
float lowpass_triceps_states[8]; //Aantal states van het filter
float xtf; //Gefilterde EMG waarde triceps
float xtfmax = 0; //Maximale gefilterde EMG waarde triceps
float xbt; //Thresholdwaarde EMG biceps
float xtt; //Thresholdwaarde EMG triceps
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; //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) //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
{
*in = max;
}
else //In alle andere gevallen is de waarde de waarde zelf
{
*in = *in;
}
}
void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie
{
error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - 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)
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 %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 (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\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
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
{
dir_motor_arm2.write(1);
}
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)
{
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
}
}
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
}
void filter_triceps() //Filtert het EMG signaal van de triceps
{
arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); //Highpass filter voor het EMG signaal van de triceps, filtert de xt en schrijft het over in de xtf, 1 EMG waarde filteren per keer
arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1); //Notch filter voor het EMG signaal van de triceps, filtert de xtf uit de highpass en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is
arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); //Lowpass filter voor het EMG signaal van de triceps, filtert de xtf uit de rectifier en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen
}
int main() //Main script
{
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
{
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
lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm
wait(2); //Twee seconden wachten
lcd.cls(); //Maak LCD scherm leeg
pc.printf("RUST afgerond\n\r"); //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\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(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
{
pc.printf("KALIBRATIE_ARM1\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(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 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
lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
arm_biquad_cascade_df1_init_f32(&highpass_biceps, 2, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
arm_biquad_cascade_df1_init_f32(¬ch_biceps, 2, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 2, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
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(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 meten van biceps
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
{
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
{
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
{
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
{
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
{
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
{
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:
{
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
lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
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(¬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(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
xt = EMG_tri.read(); //EMG meten van ticeps
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
{
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
{
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
{
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
{
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
{
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
{
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
break; //Stopt alle acties in deze case
}
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(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 = B; //Ga door naar state B
}
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:
{
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" B "); //Tekst op LCD scherm
pc.printf("B\n\r"); //Controle naar pc sturen
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
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 = TB; //Ga door naar state TB
}
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 BB:
{
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
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
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 = BBB; //Ga door naar state BBB
}
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 BT:
{
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
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
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 = BTB; //Ga door naar state BTB
}
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
}
case TB:
{
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
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
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 = TBB; //Ga door naar state TBB
}
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
}
case TT:
{
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
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
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 = TTB; //Ga door naar state TTB
}
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
}
case BBB:
{
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
{
state = BBBB; //Ga door naar state BBBB
}
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
}
case BBT:
{
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
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
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 = BBTB; //Ga door naar state BBTB
}
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
}
case BTB:
{
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
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
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 = BTBB; //Ga door naar state BTBB
}
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
}
case BTT:
{
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
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
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 = BTTB; //Ga door naar state BTTB
}
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 TBB:
{
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
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
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 = TBBB; //Ga door naar state TBBB
}
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
}
case TBT:
{
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
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
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 = 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
{
state = TTBB; //Ga door naar state TTBB
}
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:
{
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(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 = TTTB; //Ga door naar state TTTB
}
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
}
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)
{
//Motoractie
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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)
{
//Motoractie
}
break; //Stop met alle acties in deze case
}
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
}
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);
}
}
}