Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
main.cpp
- Committer:
- phgbartels
- Date:
- 2014-11-04
- Revision:
- 18:264812048611
- Parent:
- 17:dc695e4e749b
File content as of revision 18:264812048611:
/********************************************/ /* */ /* BRONCODE GROEP 7, MODULE 9, 2014 */ /* *-THE SLAP-* */ /* */ /* -Anne ten Dam */ /* -Laura de Heus */ /* -Moniek Strijdveen */ /* -Bart Arendshorst */ /* -Peter Bartels */ /********************************************/ #include "TextLCD.h" #include "mbed.h" #include "encoder.h" #include "HIDScope.h" #include "PwmOut.h" #include "arm_math.h" #include "MODSERIAL.h" /*definieren pinnen Motoren*/ #define M1_PWM PTA5 #define M1_DIR PTA4 #define M2_PWM PTC8 #define M2_DIR PTC9 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/ #define TSAMP 0.005 #define K_P (2000) #define K_I (0) #define K_D (0.1) #define K_P_motor2 (75) #define K_D_motor2 (0.01) #define I_LIMIT 100. #define lengte_arm 0.5 /* Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden. */ TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7 Encoder motor1(PTD3,PTD1); Encoder motor2(PTD5, PTD0); PwmOut pwm_motor1(M1_PWM); PwmOut pwm_motor2(M2_PWM); DigitalOut motordir1(M1_DIR); DigitalOut motordir2(M2_DIR); DigitalOut LEDGREEN(LED_GREEN); DigitalOut LEDRED(LED_RED); MODSERIAL pc(USBTX,USBRX); HIDScope scope(3); AnalogIn emg(PTB1); Timer statetimer; /* definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde */ Ticker statemachine; Ticker screen; arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz int previous_herhalingen = 0; int current_herhalingen = 0; int current_herhalingen_1 = 0; int previous_herhalingen_1 = 0; int previous_pos_motor1 = 0; int previous_pos_motor2 = 0; int current_pos_motor1; int current_pos_motor2; int EMG = 1; int aantal_pieken = 0; int doel; int doel_richting; int doel_hoogte; int puls_richting1 = 4000; int puls_richting2; int puls_richting3 = 4000; bool aanspan; void clamp(float * in, float min, float max); float pid(float setpoint, float measurement); float pid_motor2(float setpoint, float measurement); float pos_motor1_rad; float pos_motor2_rad; float PWM1_percentage = 0; float PWM2_percentage = 0; float PWM1; float PWM2; float prev_setpoint = 0; float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203}; float lowpass_1_states[4]; float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684}; float lowpass_2_states[4]; float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189}; float highpass_states[4]; float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362}; float notch_states[4]; float emg_filtered; float emg_max = 0; float emg_treshhold_laag = 0; float emg_treshhold_hoog = 0; float marge = 0; float PWM_richting1; float PWM_richting2; float PWM_richting3; float position2_setpoint; float snelheid; enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES state_t state = RUST; void rust() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; }//void rust void pieken_tellen() { if (emg_filtered>=emg_treshhold_hoog) { aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. }//if if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt aanspan=false; aantal_pieken++; doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1; }//if }//void pieken_tellen void emg_filtering() { uint16_t emg_value; float emg_value_f32; emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) emg_value_f32 = emg.read(); arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); emg_filtered = fabs(emg_filtered); arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); //scope.set(0,emg_value); //uint value //scope.set(1,emg_filtered); //processed float if(state!=EMG_KALIBRATIE) { pieken_tellen(); }//if //pc.printf("%d\n\r",doel); scope.set(0, doel); scope.set(1, aantal_pieken); scope.send(); }//void emg_filtering() void emg_max_meting() { emg_filtering(); if (emg_filtered>=emg_max) { emg_max=emg_filtered; }//if emg_treshhold_laag = 0.4*emg_max; emg_treshhold_hoog = 0.7*emg_max; }//void emg_max_meting void akkoord_geven() { emg_filtering(); } void emg_kalibratie() { //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten. current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; //} if(current_herhalingen<=1000) { emg_max_meting(); }//if }//void emg_kalibratie void arm_kalibratie() { //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop motor1.setPosition(0); //motor2.setPosition(0); akkoord_geven(); }//void arm_kalibratie void doel_bepaling() { if(200<=current_herhalingen && current_herhalingen <1200) { emg_filtering(); doel = aantal_pieken-(((aantal_pieken-1)/3)*3); }//if else if(current_herhalingen == 1200 && state==METEN_HOOGTE) { doel_hoogte = doel; aantal_pieken = 0; doel = 0; } else if(current_herhalingen == 1200 && state==METEN_RICHTING) { doel_richting = doel; aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt. doel = 0; } else if(1200<current_herhalingen && current_herhalingen<=2200) { akkoord_geven(); }//else if else { }//else }//void doel_bepaling void meten_hoogte() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; doel_bepaling(); }//void meten_hoogte void meten_richting() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; doel_bepaling(); }//void meten_richting void GotoPosition (float position_setpoint_rad, float speed_radpersecond) { static float setpoint = 0; if (setpoint < position_setpoint_rad) { setpoint += (0.005 * speed_radpersecond); if (setpoint > position_setpoint_rad) { setpoint = position_setpoint_rad; } } else if (setpoint > position_setpoint_rad) { setpoint -= (0.005 * speed_radpersecond); if (setpoint < position_setpoint_rad) { setpoint = position_setpoint_rad; } } else if (setpoint == position_setpoint_rad) { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } pc.printf("%f\r\n", setpoint); current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa PWM1_percentage = pid(setpoint, pos_motor1_rad); if (PWM1_percentage < -100) { PWM1_percentage = -100; } else if (PWM1_percentage >100) { PWM1_percentage =100; } if(PWM1_percentage < 0) { motordir1 = 1; }//if else { motordir1 = 0; }//else pwm_motor1.write(abs(PWM1_percentage/100.)); }//void GotoPosition float pid(float setpoint, float measurement) { float error; static float prev_error = 0; float out_p = 0; static float out_i = 0; float out_d = 0; error = (setpoint-measurement); out_p = error*K_P; out_i += error*K_I; out_d = (error-prev_error)*K_D; clamp(&out_i,-I_LIMIT,I_LIMIT); prev_error = error; return out_p + out_i + out_d; }//float pid void translatie (float position2_setpoint_rad, float speed2_radpersecond) { static float setpoint = 0; if (setpoint < position2_setpoint_rad) { setpoint += (0.005 * speed2_radpersecond); if (setpoint > position2_setpoint_rad) { setpoint = position2_setpoint_rad; } } else if (setpoint > position2_setpoint_rad) { setpoint -= (0.005 * speed2_radpersecond); if (setpoint < position2_setpoint_rad) { setpoint = position2_setpoint_rad; } } else if (setpoint == position2_setpoint_rad) { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad); if (PWM2_percentage < -100) { PWM2_percentage = -100; } else if (PWM2_percentage >100) { PWM2_percentage =100; } if(PWM2_percentage < 0) { motordir2 = 1; }//if else { motordir2 = 0; }//else pwm_motor2.write(abs(PWM2_percentage/100.)); prev_setpoint = setpoint; } float pid_motor2(float setpoint, float measurement) { float error; static float prev_error = 0; float out_p = 0; float out_d = 0; error = (setpoint-measurement); out_p = error*K_P_motor2; out_d = (error-prev_error)*K_D_motor2; prev_error = error; return out_p + out_d; }//float pid void clamp(float* in, float min, float max) { *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; }//void clamp void statemachinefunction() { statetimer.reset(); //pc.printf("."); switch(state) { case RUST: { rust(); /*voorwaarde wanneer hij door kan naar de volgende case*/ if (current_herhalingen == 100 && EMG == 1) { current_herhalingen = 0; previous_herhalingen = 0; state = EMG_KALIBRATIE; EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten }//if (current_herhalingen == 100 && EMG == 1) else if(current_herhalingen == 100) { current_herhalingen = 0; previous_herhalingen = 0; state = ARM_KALIBRATIE; }//else break; }//case RUST: case EMG_KALIBRATIE: { emg_kalibratie(); if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/ current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; state = ARM_KALIBRATIE; }//if (current_herhalingen >=1000) break; }//case EMG_KALIBRATIE case ARM_KALIBRATIE: { arm_kalibratie(); if (aantal_pieken == 1) { current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; state = METEN_HOOGTE; }//if (current_herhalingen == 100) break; }//case ARM_KALIBRATIE: case METEN_HOOGTE: { meten_hoogte(); if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) { current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; //doel_hoogte = 0; state = METEN_RICHTING; }//if (current_herhalingen == 2800 && aantal_pieken == 1) else if (current_herhalingen == 2200) { current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; state = METEN_HOOGTE; }///else break; }//case METEN_HOOGTE case METEN_RICHTING: { meten_richting(); if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) { current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; state = INSTELLEN_RICHTING; }//if (current_herhalingen == 2800 && aantal_pieken == 1) else if (current_herhalingen == 2200) { current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; state = METEN_RICHTING; }///else break; }//case METEN_RICHTING case TEST: { state = RUST; break; } case INSTELLEN_RICHTING: { //instellen_richting(); if (doel_richting == 1) { position2_setpoint = 0; } else if (doel_richting ==2) { position2_setpoint = 14.5; } else { position2_setpoint = 25; } translatie(position2_setpoint, 1); if (current_herhalingen >= 400) { current_herhalingen = 0; previous_herhalingen = 0; doel_richting = 0; state = SLAAN; }//if (current_herhalingen == 100 break; }//case INSTELLEN_RICHTING case SLAAN: { if (doel_hoogte == 1) { snelheid = 2; } else if (doel_hoogte ==2) { snelheid = 2.25; } else { snelheid = 5; } GotoPosition(1.9, snelheid); if (current_herhalingen == 400) { current_herhalingen = 0; previous_herhalingen = 0; prev_setpoint =0; //setpoint = 0; state = RETURN2RUST; }//if (current_herhalingen == 100) break; }//case SLAAN case RETURN2RUST: { GotoPosition(0,0.1); doel_richting = 0; doel_hoogte = 0; if (current_herhalingen >= 200) { current_herhalingen = 0; translatie(0,1.6); if (current_herhalingen >=200){ state = RUST; current_herhalingen = 0; previous_herhalingen = 0; current_herhalingen = 0; current_herhalingen = 0; } }//if (current_herhalingen == 100) break; }// case RETURN2RUST default: { state = RUST; }//default }//switch(state) }//void statemachinefunction void screenupdate() { if(state==RUST) { lcd.cls(); lcd.locate(0,0); lcd.printf("V.I.C.T.O.R.Y."); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(" GROEP 7 "); }//if(state==RUST) else if(state==EMG_KALIBRATIE) { lcd.cls(); lcd.locate(0,0); lcd.printf("Max. aanspannen"); if(current_herhalingen<=200) { lcd.locate(0,1); lcd.printf("nog 5 sec."); }//if(current_herhalingen<=200) else if(current_herhalingen<=400) { lcd.locate(0,1); lcd.printf("nog 4 sec."); }//else if(current_herhalingen<=400) else if(current_herhalingen<=600) { lcd.locate(0,1); lcd.printf("nog 3 sec."); }//else if(current_herhalingen<=600) else if(current_herhalingen<=800) { lcd.locate(0,1); lcd.printf("nog 2 sec."); }//else if(current_herhalingen<=800) else if(current_herhalingen<=1000) { lcd.locate(0,1); lcd.printf("nog 1 sec."); }//else if(current_herhalingen<=1000) }//else if(state==EMG_KALIBRATIE) else if(state==ARM_KALIBRATIE) { lcd.cls(); lcd.locate(0,0); lcd.printf("Set arm to zero"); lcd.locate(0,1); lcd.printf("Klaar? Span aan"); }//else if(state==ARM_KALIBRATIE) else if(state==METEN_HOOGTE) { lcd.cls(); if(current_herhalingen<=200) { lcd.locate(0,0); lcd.printf("Hoogte bepalen:"); lcd.locate(0,1); lcd.printf("span aan per vak"); }//if(current_herhalingen<=200){ else if(200<=current_herhalingen && current_herhalingen<1200) { lcd.locate(0,0); lcd.printf("Vak %d",doel); if(current_herhalingen<=400) { lcd.locate(0,1); lcd.printf("nog 5 sec."); }//if(current_herhalingen<=400) else if(current_herhalingen<=600) { lcd.locate(0,1); lcd.printf("nog 4 sec."); }//else if(current_herhalingen<=600) else if(current_herhalingen<=800) { lcd.locate(0,1); lcd.printf("nog 3 sec."); }//else if(current_herhalingen<=800) else if(current_herhalingen<=1000) { lcd.locate(0,1); lcd.printf("nog 2 sec."); }//else if(current_herhalingen<=1000) else if(current_herhalingen<1200) { lcd.locate(0,1); lcd.printf("nog 1 sec."); }//else if(current_herhalingen<=1200) }//else if(200<=current_herhalingen<=1200) else if(current_herhalingen<=2200) { lcd.locate(0,0); lcd.printf("Vak %d akkoord?",doel_hoogte); lcd.locate(0,1); lcd.printf("Span aan"); }//else if(current_herhalingen<=1600){ else { lcd.locate(0,0); lcd.printf("Opnieuw hoogte"); lcd.locate(0,1); lcd.printf("bepalen"); }//else{ }//else if(state==METEN_HOOGTE){ else if(state==METEN_RICHTING) { lcd.cls(); if(current_herhalingen<=200) { lcd.locate(0,0); lcd.printf("Richting bepalen:"); lcd.locate(0,1); lcd.printf("span aan per vak"); }//if(current_herhalingen<=200) else if(200<=current_herhalingen && current_herhalingen<1200) { lcd.locate(0,0); lcd.printf("Vak %d",doel); if(current_herhalingen<=400) { lcd.locate(0,1); lcd.printf("nog 5 sec."); }//if(current_herhalingen<=400) else if(current_herhalingen<=600) { lcd.locate(0,1); lcd.printf("nog 4 sec."); }//else if(current_herhalingen<=600) else if(current_herhalingen<=800) { lcd.locate(0,1); lcd.printf("nog 3 sec."); }//else if(current_herhalingen<=800) else if(current_herhalingen<=1000) { lcd.locate(0,1); lcd.printf("nog 2 sec."); }//else if(current_herhalingen<=1000) else if(current_herhalingen<1200) { lcd.locate(0,1); lcd.printf("nog 1 sec."); }//else if(current_herhalingen<=1200) }//else if(200<=current_herhalingen<=1200) else if(current_herhalingen<=2200) { lcd.locate(0,0); lcd.printf("Vak %d akkoord?",doel_richting); lcd.locate(0,1); lcd.printf("Span aan"); }//else if(current_herhalingen<=1600) else { lcd.locate(0,0); lcd.printf("Opnieuw richting"); lcd.locate(0,1); lcd.printf("bepalen"); }//else }//else if(state==METEN_RICHTING){ else if(state==INSTELLEN_RICHTING) { lcd.cls(); lcd.locate(0,0); lcd.printf("Instellen hoek"); lcd.locate(0,1); lcd.printf("Even wachten..."); }//else if(state==INSTELLEN_RICHTING){ else if(state==SLAAN) { lcd.cls(); lcd.locate(0,0); lcd.printf("Slaan, pas op"); lcd.locate(0,1); lcd.printf("Let's pray"); }//else if(state==INSTELLEN_RICHTING){ else if(state==RETURN2RUST) { lcd.cls(); lcd.locate(0,0); lcd.printf("Terug naar"); lcd.locate(0,1); lcd.printf("0-positie"); }//else if(state==INSTELLEN_RICHTING){ else { lcd.cls(); lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen. }//else{ } int main() { pwm_motor1.period_us(100); pwm_motor2.period_us(100); pc.baud(115200); statetimer.start(); arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states); arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states); arm_biquad_cascade_df1_init_f32(¬ch,1 , notch_const, notch_states); arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states); state = TEST; statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds) //screen.attach(&screenupdate, 0.2); while(1) { screenupdate(); wait(0.2); }; }