Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
main.cpp
- Committer:
- lauradeheus
- Date:
- 2014-11-01
- Revision:
- 8:f733c6a27c15
- Parent:
- 7:7e3e183bf063
- Child:
- 9:7e9b63fe8988
File content as of revision 8:f733c6a27c15:
/********************************************/ /* */ /* 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" /*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 PI 3.1415926535897 #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); Serial 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 previous_pos_motor1 = 0; int current_pos_motor1; int EMG = 1; int aantal_pieken; int doel; int doel_richting; int doel_hoogte; 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; 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/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 scope.set(2,doel); scope.send(); if(state!=EMG_KALIBRATIE) { pieken_tellen(); }//if }//void emg_filtering() void emg_max_meting(){ emg_filtering(); if (emg_filtered>=emg_max) { emg_max=emg_filtered; }//if emg_treshhold_laag = 0.3*emg_max; emg_treshhold_hoog = 0.7*emg_max; }//void emg_max_meting 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 current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; motor1.setPosition(0); motor2.setPosition(0); }//void arm_kalibratie void doel_bepaling() { if(200<=current_herhalingen<=1200){ emg_filtering(); }//if else if(1600<=current_herhalingen<=2200){ emg_filtering(); }//else if else{ }//else }//void doel_bepaling void meten_hoogte() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; doel_bepaling(); if(1600<=current_herhalingen<=2200 && aantal_pieken==1){ doel=doel_hoogte; }//if }//void meten_hoogte void meten_richting() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; doel_bepaling(); if(1600<=current_herhalingen<=2200 && aantal_pieken==1){ doel=doel_richting; }//if }//void meten_richting void instellen_richting() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; }//void instellen_richting void GotoPosition (float position_setpoint_rad, float speed_radpersecond) { 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 positie regelen i.p.v. snelheid. if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //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() { 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 = ARM_KALIBRATIE; EMG = 0; }//if (current_herhalingen == 100 && EMG == 1) break; }//case RUST: case ARM_KALIBRATIE: { arm_kalibratie(); if (current_herhalingen == 100) { current_herhalingen = 0; previous_herhalingen = 0; state = EMG_KALIBRATIE; motor1.setPosition(0); motor2.setPosition(0); pwm_motor1.period_us(100); pwm_motor2.period_us(100); }//if (current_herhalingen == 100) break; }//case ARM_KALIBRATIE: case EMG_KALIBRATIE: { emg_kalibratie(); if (current_herhalingen >=1000) { state = METEN_HOOGTE; current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; }//if (current_herhalingen >=1000) break; }//case EMG_KALIBRATIE case METEN_HOOGTE: { meten_hoogte(); if (current_herhalingen == 2800 && aantal_pieken == 1) { state = METEN_RICHTING; current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; }//if (current_herhalingen == 2800 && aantal_pieken == 1) else { state = METEN_HOOGTE; current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; }///else break; }//case METEN_HOOGTE case METEN_RICHTING: { meten_richting(); if (current_herhalingen == 2800 && aantal_pieken == 1) { state = INSTELLEN_RICHTING; current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; }//if (current_herhalingen == 2800 && aantal_pieken == 1) else { state = METEN_RICHTING; current_herhalingen = 0; previous_herhalingen = 0; aantal_pieken = 0; doel = 0; }//else break; }//case METEN_RICHTING case INSTELLEN_RICHTING: { instellen_richting(); if (current_herhalingen == 100) { state = SLAAN; current_herhalingen = 0; previous_herhalingen = 0; }//if (current_herhalingen == 100) break; }//case INSTELLEN_RICHTING case SLAAN: { GotoPosition(1.5 ,8, 0.1); if (current_herhalingen == 100) { state = RETURN2RUST; current_herhalingen = 0; previous_herhalingen = 0; prev_setpoint =0; setpoint =0; }//if (current_herhalingen == 100) break; }//case SLAAN case RETURN2RUST: { GotoPosition(0,4, 0.05); //if (current_herhalingen == 100) //{ // state = RUST; // current_herhalingen = 0; // previous_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==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<=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<=1600){ lcd.locate(0,0); lcd.printf("Vak %d akkoord?",doel); lcd.locate(0,1); lcd.printf("Span aan"); }//else if(current_herhalingen<=1600){ else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){ lcd.locate(0,0); lcd.printf("Vak %d akkoord",doel_hoogte); }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){ 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<=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<=1600){ lcd.locate(0,0); lcd.printf("Vak %d akkoord?",doel); lcd.locate(0,1); lcd.printf("Span aan"); }//else if(current_herhalingen<=1600) else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){ lcd.locate(0,0); lcd.printf("Vak %d akkoord",doel_richting); }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1) else{ lcd.locate(0,0); lcd.printf("Opnieuw richting"); lcd.locate(0,1); lcd.printf("bepalen"); }//else }//else if(state==METEN_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); }