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-03
- Revision:
- 11:687aa4656a6e
- Parent:
- 10:cd89569cd847
- Child:
- 12:3fec73bc3318
File content as of revision 11:687aa4656a6e:
/********************************************/ /* */ /* 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 (80000) #define K_I (0.01) #define K_D (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); /* 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 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; int puls_richting2; int puls_richting3; bool aanspan; void clamp(float * in, float min, float max); float pid(float setpoint, float measurement); float pos_motor1_rad; float PWM1_percentage = 0; float PWM1; float PWM2; float setpoint = 0; 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; enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //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); pwm_motor1.period_us(100); pwm_motor2.period_us(100); 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 instellen_richting() { current_pos_motor2 = motor2.getPosition(); pc.printf("%d\n\r", current_pos_motor2); /* if (doel_richting ==1){ if (state==RETURN2RUST){ motordir2 = 1; while (current_pos_motor2 > 0){ pwm_motor2.write(PWM_richting1); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//if (doel_richting == 1) else{ motordir2 = 0; while (current_pos_motor2 < puls_richting1){ pwm_motor2.write(PWM_richting1); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//else }//if (doel_richting ==1) else if (doel_richting == 2){ if (state==RETURN2RUST){ motordir2 = 1; while (current_pos_motor2 > 0){ pwm_motor2.write(PWM_richting2); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//if (doel_richting == 1) else{ motordir2 = 0; while (current_pos_motor2 < puls_richting2){ pwm_motor2.write(PWM_richting2); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//if (doel_richting == 1) }//if (doel_richting ==1) else if(doel_richting == 3){ if (state==RETURN2RUST){ motordir2 =1; while (current_pos_motor2 > 0){ pwm_motor2.write(PWM_richting3); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//if (doel_richting == 1) else{ motordir2 = 0; while (current_pos_motor2 < puls_richting3){ pwm_motor2.write(PWM_richting3); }//while (current_pos_motor2 < rad_richting1) current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; }//if (doel_richting == 1) }//if (doel_richting ==1) */ }//void instellen_richting void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge) { 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 regelaar previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. //nu gaan we snelheid volgen d.m.v. positie regeling if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) { //if position error < ...rad, then stop. speed_radpersecond = 0; setpoint = pos_motor1_rad; current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; //pc.printf("stop\n\r"); PWM1_percentage = 0; }//if else if(pos_motor1_rad - position_setpoint_rad < 0) { setpoint = prev_setpoint +( TSAMP * speed_radpersecond); PWM1_percentage = pid(setpoint, pos_motor1_rad); }//else if else { setpoint = prev_setpoint -( TSAMP * speed_radpersecond); PWM1_percentage = pid(setpoint, pos_motor1_rad); }//else //pc.printf("%f\n\r",PWM1_percentage); if (PWM1_percentage < -100) { PWM1_percentage = -100; }//if else if (PWM1_percentage >100) { PWM1_percentage =100; }//else if if(PWM1_percentage < 0) { motordir1 = 1; }//if else { motordir1 = 0; }//else pwm_motor1.write(abs(PWM1_percentage/100.)); prev_setpoint = setpoint; }//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 clamp(float* in, float min, float max) { *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; }//void clamp void statemachinefunction() { 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 = METEN_HOOGTE; }//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 INSTELLEN_RICHTING: { instellen_richting(); /*if (current_herhalingen == 100) { current_herhalingen_1 = 0; previous_herhalingen_1 = 0; doel_richting = 0; state = SLAAN; }//if (current_herhalingen == 100) */ break; }//case INSTELLEN_RICHTING case SLAAN: { GotoPosition(1.5 ,8, 0.1); 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: { instellen_richting(); GotoPosition(0,4, 0.05); doel_richting = 0; doel_hoogte = 0; if (current_herhalingen >= 200 && current_herhalingen_1 >= 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() { pc.printf("l"); 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) { pc.printf("i"); lcd.cls(); lcd.locate(0,0); lcd.printf("Instellen hoek"); lcd.locate(0,1); lcd.printf("Even "); pc.printf("e"); }//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() { pc.baud(115200); 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); 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); }