main.cpp

00001 /********************************************/
00002 /*                                          */
00003 /*   BRONCODE GROEP 7, MODULE 9, 2014       */
00004 /*       *-THE SLAP-*                       */
00005 /*                                          */
00006 /* -Anne ten Dam                            */
00007 /* -Laura de Heus                           */
00008 /* -Moniek Strijdveen                       */
00009 /* -Bart Arendshorst                        */
00010 /* -Peter Bartels                           */
00011 /********************************************/
00012 
00013 #include "TextLCD.h"
00014 #include "mbed.h"
00015 #include "encoder.h"
00016 #include "HIDScope.h"
00017 #include "PwmOut.h"
00018 #include "arm_math.h"
00019 #include "MODSERIAL.h"
00020 
00021 /*definieren pinnen Motoren*/
00022 #define M1_PWM PTA5
00023 #define M1_DIR PTA4
00024 #define M2_PWM PTC8
00025 #define M2_DIR PTC9
00026 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
00027 #define TSAMP 0.005
00028 #define K_P (5000)
00029 #define K_I (0)
00030 #define K_D (0.1)
00031 #define K_P_motor2 (75)
00032 #define K_D_motor2 (0.01)
00033 #define I_LIMIT 100.
00034 #define lengte_arm 0.5
00035 
00036 /*
00037 Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
00038 */
00039 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
00040 Encoder motor1(PTD3,PTD1);
00041 Encoder motor2(PTD5, PTD0);
00042 PwmOut pwm_motor1(M1_PWM);
00043 PwmOut pwm_motor2(M2_PWM);
00044 DigitalOut motordir1(M1_DIR);
00045 DigitalOut motordir2(M2_DIR);
00046 DigitalOut LEDGREEN(LED_GREEN);
00047 DigitalOut LEDRED(LED_RED);
00048 MODSERIAL pc(USBTX,USBRX);
00049 HIDScope scope(3);
00050 AnalogIn emg(PTB1);
00051 
00052 Timer statetimer;
00053 /*
00054 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
00055 */
00056 Ticker statemachine;
00057 Ticker screen;
00058 arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz
00059 arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz
00060 arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz
00061 arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
00062 int previous_herhalingen = 0;
00063 int current_herhalingen = 0;
00064 int current_herhalingen_1 = 0;
00065 int previous_herhalingen_1 = 0;
00066 int previous_pos_motor1 = 0;
00067 int previous_pos_motor2 = 0;
00068 int current_pos_motor1;
00069 int current_pos_motor2;
00070 int EMG = 1;
00071 int aantal_pieken = 0;
00072 int doel;
00073 int doel_richting;
00074 int doel_hoogte;
00075 int puls_richting1 = 4000;
00076 int puls_richting2;
00077 int puls_richting3 = 4000;
00078 bool aanspan;
00079 void clamp(float * in, float min, float max);
00080 float pid(float setpoint, float measurement);
00081 float pid_motor2(float setpoint, float measurement);
00082 float pos_motor1_rad;
00083 float pos_motor2_rad;
00084 float PWM1_percentage = 0;
00085 float PWM2_percentage = 0;
00086 float PWM1;
00087 float PWM2;
00088 float prev_setpoint = 0;
00089 float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
00090 float lowpass_1_states[4];
00091 float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
00092 float lowpass_2_states[4];
00093 float highpass_const[] = {0.638945525159022 , -1.277891050318045 ,  0.638945525159022 , 1.142980502539901 , -0.412801598096189};
00094 float highpass_states[4];
00095 float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
00096 float notch_states[4];
00097 float emg_filtered;
00098 float emg_max = 0;
00099 float emg_treshhold_laag = 0;
00100 float emg_treshhold_hoog = 0;
00101 float marge = 0;
00102 float PWM_richting1;
00103 float PWM_richting2;
00104 float PWM_richting3;
00105 float position2_setpoint;
00106 
00107 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
00108 state_t state = RUST;
00109 
00110 void rust()
00111 {
00112     current_herhalingen = previous_herhalingen + 1;
00113     previous_herhalingen = current_herhalingen;
00114 }//void rust
00115 
00116 void pieken_tellen()
00117 {
00118     if (emg_filtered>=emg_treshhold_hoog) {
00119         aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
00120     }//if
00121     if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
00122         aanspan=false;
00123         aantal_pieken++;
00124         doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
00125 
00126     }//if
00127 }//void pieken_tellen
00128 
00129 void emg_filtering()
00130 {
00131     uint16_t emg_value;
00132     float emg_value_f32;
00133     emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
00134     emg_value_f32 = emg.read();
00135 
00136     arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
00137     arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
00138     arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
00139     emg_filtered = fabs(emg_filtered);
00140     arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
00141     //scope.set(0,emg_value);     //uint value
00142     //scope.set(1,emg_filtered);  //processed float
00143     if(state!=EMG_KALIBRATIE) {
00144         pieken_tellen();
00145     }//if
00146     //pc.printf("%d\n\r",doel);
00147     scope.set(0, doel);
00148     scope.set(1, aantal_pieken);
00149     scope.send();
00150 }//void emg_filtering()
00151 
00152 void emg_max_meting()
00153 {
00154     emg_filtering();
00155     if (emg_filtered>=emg_max) {
00156         emg_max=emg_filtered;
00157     }//if
00158     emg_treshhold_laag = 0.4*emg_max;
00159     emg_treshhold_hoog = 0.7*emg_max;
00160 }//void emg_max_meting
00161 
00162 void akkoord_geven()
00163 {
00164     emg_filtering();
00165 }
00166 
00167 void emg_kalibratie()
00168 {
00169     //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.
00170     current_herhalingen = previous_herhalingen + 1;
00171     previous_herhalingen = current_herhalingen;
00172     //}
00173     if(current_herhalingen<=1000) {
00174         emg_max_meting();
00175     }//if
00176 }//void emg_kalibratie
00177 
00178 void arm_kalibratie()
00179 {
00180     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
00181     motor1.setPosition(0);
00182     //motor2.setPosition(0);
00183     akkoord_geven();
00184 }//void arm_kalibratie
00185 
00186 void doel_bepaling()
00187 {
00188     if(200<=current_herhalingen && current_herhalingen <1200) {
00189         emg_filtering();
00190         doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
00191     }//if
00192     else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
00193         doel_hoogte = doel;
00194         aantal_pieken = 0;
00195         doel = 0;
00196     } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
00197         doel_richting = doel;
00198         aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
00199         doel = 0;
00200     } else if(1200<current_herhalingen && current_herhalingen<=2200) {
00201         akkoord_geven();
00202     }//else if
00203     else {
00204     }//else
00205 }//void doel_bepaling
00206 
00207 void meten_hoogte()
00208 {
00209     current_herhalingen = previous_herhalingen + 1;
00210     previous_herhalingen = current_herhalingen;
00211     doel_bepaling();
00212 }//void meten_hoogte
00213 
00214 void meten_richting()
00215 {
00216     current_herhalingen = previous_herhalingen + 1;
00217     previous_herhalingen = current_herhalingen;
00218     doel_bepaling();
00219 }//void meten_richting
00220 
00221 void GotoPosition (float position_setpoint_rad, float speed_radpersecond)
00222 {
00223     static float setpoint = 0; 
00224     if (setpoint < position_setpoint_rad) {
00225         setpoint += (TSAMP * speed_radpersecond);
00226         if (setpoint > position_setpoint_rad) {
00227             setpoint = position_setpoint_rad;
00228         }
00229     }
00230 
00231     else if (setpoint > position_setpoint_rad) {
00232         setpoint -= (TSAMP * speed_radpersecond);
00233         if (setpoint < position_setpoint_rad) {
00234             setpoint = position_setpoint_rad;
00235         }
00236     } else if (setpoint == position_setpoint_rad) {
00237         current_herhalingen = previous_herhalingen + 1;
00238         previous_herhalingen = current_herhalingen;
00239     }
00240 
00241     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00242     pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
00243     PWM1_percentage = pid(setpoint, pos_motor1_rad);
00244     pc.printf("setpoint : %f\r\n",setpoint);
00245     pc.printf("pos_motor_1 :%f\r\n", pos_motor1_rad);
00246 
00247     if (PWM1_percentage < -100) {
00248         PWM1_percentage = -100;
00249     } else if (PWM1_percentage >100) {
00250         PWM1_percentage =100;
00251     }
00252 
00253     if(PWM1_percentage < 0) {
00254         motordir1 = 1;
00255     }//if
00256     else {
00257         motordir1 = 0;
00258     }//else
00259 
00260     pwm_motor1.write(abs(PWM1_percentage/100.));
00261 }//void GotoPosition
00262 
00263 float pid(float setpoint, float measurement)
00264 {
00265     float error;
00266     static float prev_error = 0;
00267     float           out_p = 0;
00268     static float    out_i = 0;
00269     float           out_d = 0;
00270     error  = (setpoint-measurement);
00271     out_p  = error*K_P;
00272     out_i += error*K_I;
00273     out_d  = (error-prev_error)*K_D;
00274     clamp(&out_i,-I_LIMIT,I_LIMIT);
00275     prev_error = error;
00276     return out_p + out_i + out_d;
00277 }//float pid
00278 
00279 void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2)
00280 {
00281     static float setpoint = 0; 
00282     if (setpoint < position2_setpoint_rad) {
00283         setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond);
00284         if (setpoint > position2_setpoint_rad) {
00285             setpoint = position2_setpoint_rad;
00286         }
00287     }
00288 
00289     else if (setpoint > position2_setpoint_rad) {
00290         setpoint = prev_setpoint - (0.0005 * speed2_radpersecond);
00291         if (setpoint < position2_setpoint_rad) {
00292             setpoint = position2_setpoint_rad;
00293         }
00294     } else if (setpoint == position2_setpoint_rad) {
00295         current_herhalingen = previous_herhalingen + 1;
00296         previous_herhalingen = current_herhalingen;
00297     }
00298 
00299     current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00300     pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2);
00301     pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
00302     PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);
00303 
00304     if (PWM2_percentage < -100) {
00305         PWM2_percentage = -100;
00306     } else if (PWM2_percentage >100) {
00307         PWM2_percentage =100;
00308     }
00309 
00310     if(PWM2_percentage < 0) {
00311         motordir2 = 1;
00312     }//if
00313     else {
00314         motordir2 = 0;
00315     }//else
00316 
00317     pwm_motor2.write(abs(PWM2_percentage/100.));
00318     prev_setpoint = setpoint;
00319 }
00320 
00321 float pid_motor2(float setpoint, float measurement)
00322 {
00323     float error;
00324     static float prev_error = 0;
00325     float           out_p = 0;
00326     float           out_d = 0;
00327     error  = (setpoint-measurement);
00328     out_p  = error*K_P_motor2;
00329     out_d  = (error-prev_error)*K_D_motor2;
00330     prev_error = error;
00331     return out_p + out_d;
00332 }//float pid
00333 
00334 void clamp(float* in, float min, float max)
00335 {
00336 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
00337 }//void clamp
00338 
00339 void statemachinefunction()
00340 {
00341     statetimer.reset();
00342     //pc.printf(".");
00343     switch(state) {
00344         case RUST: {
00345             rust();
00346             /*voorwaarde wanneer hij door kan naar de volgende case*/
00347             if (current_herhalingen == 100 && EMG == 1) {
00348                 current_herhalingen = 0;
00349                 previous_herhalingen = 0;
00350                 state = EMG_KALIBRATIE;
00351                 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
00352             }//if (current_herhalingen == 100 && EMG == 1)
00353             else if(current_herhalingen == 100) {
00354                 current_herhalingen = 0;
00355                 previous_herhalingen = 0;
00356                 state = METEN_HOOGTE;
00357             }//else
00358             break;
00359         }//case RUST:
00360 
00361         case EMG_KALIBRATIE: {
00362             emg_kalibratie();
00363             if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
00364                 current_herhalingen = 0;
00365                 previous_herhalingen = 0;
00366                 aantal_pieken = 0;
00367                 doel = 0;
00368                 state = ARM_KALIBRATIE;
00369             }//if (current_herhalingen >=1000)
00370             break;
00371         }//case EMG_KALIBRATIE
00372 
00373         case ARM_KALIBRATIE: {
00374             arm_kalibratie();
00375             if (aantal_pieken == 1) {
00376                 current_herhalingen = 0;
00377                 previous_herhalingen = 0;
00378                 aantal_pieken = 0;
00379                 doel = 0;
00380                 state = METEN_HOOGTE;
00381             }//if (current_herhalingen == 100)
00382             break;
00383         }//case ARM_KALIBRATIE:
00384 
00385         case METEN_HOOGTE: {
00386             meten_hoogte();
00387             if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
00388                 current_herhalingen = 0;
00389                 previous_herhalingen = 0;
00390                 aantal_pieken = 0;
00391                 doel = 0;
00392                 //doel_hoogte = 0;
00393                 state = METEN_RICHTING;
00394             }//if (current_herhalingen == 2800 && aantal_pieken == 1)
00395             else if (current_herhalingen == 2200) {
00396                 current_herhalingen = 0;
00397                 previous_herhalingen = 0;
00398                 aantal_pieken = 0;
00399                 doel = 0;
00400                 state = METEN_HOOGTE;
00401             }///else
00402             break;
00403         }//case METEN_HOOGTE
00404 
00405         case METEN_RICHTING: {
00406             meten_richting();
00407             if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
00408                 current_herhalingen = 0;
00409                 previous_herhalingen = 0;
00410                 aantal_pieken = 0;
00411                 doel = 0;
00412                 state = INSTELLEN_RICHTING;
00413             }//if (current_herhalingen == 2800 && aantal_pieken == 1)
00414             else if (current_herhalingen == 2200) {
00415                 current_herhalingen = 0;
00416                 previous_herhalingen = 0;
00417                 aantal_pieken = 0;
00418                 doel = 0;
00419                 state = METEN_RICHTING;
00420             }///else
00421             break;
00422         }//case METEN_RICHTING
00423 
00424 
00425         case TEST: {
00426             state = SLAAN;
00427             break;
00428         }
00429 
00430         case INSTELLEN_RICHTING: {
00431             //instellen_richting();
00432             if (doel_richting == 1) {
00433                 position2_setpoint = 0;
00434             } else if (doel_richting ==2) {
00435                 position2_setpoint = 14.5;
00436             } else {
00437                 position2_setpoint = 25;
00438             }
00439 
00440 
00441             translatie(position2_setpoint, 10, 0.1);
00442 
00443             if (current_herhalingen >= 400) {
00444                 current_herhalingen = 0;
00445                 previous_herhalingen = 0;
00446                 doel_richting = 0; 
00447                 state = RETURN2RUST;
00448             }//if (current_herhalingen == 100
00449             break;
00450         }//case INSTELLEN_RICHTING
00451 
00452         case SLAAN: {
00453             GotoPosition(1.5 ,8);
00454             if (current_herhalingen == 400) {
00455                 current_herhalingen = 0;
00456                 previous_herhalingen = 0;
00457                 prev_setpoint =0;
00458                 //setpoint = 0;
00459                 state = RETURN2RUST;
00460             }//if (current_herhalingen == 100)
00461             break;
00462         }//case SLAAN
00463 
00464         case RETURN2RUST: {
00465             //translatie(0,10,0.1);
00466             GotoPosition(0,1);
00467             doel_richting = 0;
00468             doel_hoogte = 0;
00469             if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) {
00470                 state = RUST;
00471                 current_herhalingen = 0;
00472                 previous_herhalingen = 0;
00473                 current_herhalingen = 0;
00474                 current_herhalingen = 0;
00475             }//if (current_herhalingen == 100)
00476             
00477             break;
00478         }// case RETURN2RUST
00479 
00480         default: {
00481             state = RUST;
00482         }//default
00483 
00484     }//switch(state)
00485 }//void statemachinefunction
00486 
00487 
00488 void screenupdate()
00489 {
00490     if(state==RUST) {
00491         lcd.cls();
00492         lcd.locate(0,0);
00493         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
00494         lcd.locate(0,1);
00495         lcd.printf("  GROEP 7   ");
00496     }//if(state==RUST)
00497 
00498     else if(state==EMG_KALIBRATIE) {
00499         lcd.cls();
00500         lcd.locate(0,0);
00501         lcd.printf("Max. aanspannen");
00502         if(current_herhalingen<=200) {
00503             lcd.locate(0,1);
00504             lcd.printf("nog 5 sec.");
00505         }//if(current_herhalingen<=200)
00506         else if(current_herhalingen<=400) {
00507             lcd.locate(0,1);
00508             lcd.printf("nog 4 sec.");
00509         }//else if(current_herhalingen<=400)
00510         else if(current_herhalingen<=600) {
00511             lcd.locate(0,1);
00512             lcd.printf("nog 3 sec.");
00513         }//else if(current_herhalingen<=600)
00514         else if(current_herhalingen<=800) {
00515             lcd.locate(0,1);
00516             lcd.printf("nog 2 sec.");
00517         }//else if(current_herhalingen<=800)
00518         else if(current_herhalingen<=1000) {
00519             lcd.locate(0,1);
00520             lcd.printf("nog 1 sec.");
00521         }//else if(current_herhalingen<=1000)
00522     }//else if(state==EMG_KALIBRATIE)
00523 
00524     else if(state==ARM_KALIBRATIE) {
00525         lcd.cls();
00526         lcd.locate(0,0);
00527         lcd.printf("Set arm to zero");
00528         lcd.locate(0,1);
00529         lcd.printf("Klaar? Span aan");
00530     }//else if(state==ARM_KALIBRATIE)
00531 
00532     else if(state==METEN_HOOGTE) {
00533         lcd.cls();
00534         if(current_herhalingen<=200) {
00535             lcd.locate(0,0);
00536             lcd.printf("Hoogte bepalen:");
00537             lcd.locate(0,1);
00538             lcd.printf("span aan per vak");
00539         }//if(current_herhalingen<=200){
00540         else if(200<=current_herhalingen && current_herhalingen<1200) {
00541             lcd.locate(0,0);
00542             lcd.printf("Vak %d",doel);
00543             if(current_herhalingen<=400) {
00544                 lcd.locate(0,1);
00545                 lcd.printf("nog 5 sec.");
00546             }//if(current_herhalingen<=400)
00547             else if(current_herhalingen<=600) {
00548                 lcd.locate(0,1);
00549                 lcd.printf("nog 4 sec.");
00550             }//else if(current_herhalingen<=600)
00551             else if(current_herhalingen<=800) {
00552                 lcd.locate(0,1);
00553                 lcd.printf("nog 3 sec.");
00554             }//else if(current_herhalingen<=800)
00555             else if(current_herhalingen<=1000) {
00556                 lcd.locate(0,1);
00557                 lcd.printf("nog 2 sec.");
00558             }//else if(current_herhalingen<=1000)
00559             else if(current_herhalingen<1200) {
00560                 lcd.locate(0,1);
00561                 lcd.printf("nog 1 sec.");
00562             }//else if(current_herhalingen<=1200)
00563         }//else if(200<=current_herhalingen<=1200)
00564         else if(current_herhalingen<=2200) {
00565             lcd.locate(0,0);
00566             lcd.printf("Vak %d akkoord?",doel_hoogte);
00567             lcd.locate(0,1);
00568             lcd.printf("Span aan");
00569         }//else if(current_herhalingen<=1600){
00570         else {
00571             lcd.locate(0,0);
00572             lcd.printf("Opnieuw hoogte");
00573             lcd.locate(0,1);
00574             lcd.printf("bepalen");
00575         }//else{
00576     }//else if(state==METEN_HOOGTE){
00577 
00578     else if(state==METEN_RICHTING) {
00579         lcd.cls();
00580         if(current_herhalingen<=200) {
00581             lcd.locate(0,0);
00582             lcd.printf("Richting bepalen:");
00583             lcd.locate(0,1);
00584             lcd.printf("span aan per vak");
00585         }//if(current_herhalingen<=200)
00586         else if(200<=current_herhalingen && current_herhalingen<1200) {
00587             lcd.locate(0,0);
00588             lcd.printf("Vak %d",doel);
00589             if(current_herhalingen<=400) {
00590                 lcd.locate(0,1);
00591                 lcd.printf("nog 5 sec.");
00592             }//if(current_herhalingen<=400)
00593             else if(current_herhalingen<=600) {
00594                 lcd.locate(0,1);
00595                 lcd.printf("nog 4 sec.");
00596             }//else if(current_herhalingen<=600)
00597             else if(current_herhalingen<=800) {
00598                 lcd.locate(0,1);
00599                 lcd.printf("nog 3 sec.");
00600             }//else if(current_herhalingen<=800)
00601             else if(current_herhalingen<=1000) {
00602                 lcd.locate(0,1);
00603                 lcd.printf("nog 2 sec.");
00604             }//else if(current_herhalingen<=1000)
00605             else if(current_herhalingen<1200) {
00606                 lcd.locate(0,1);
00607                 lcd.printf("nog 1 sec.");
00608             }//else if(current_herhalingen<=1200)
00609         }//else if(200<=current_herhalingen<=1200)
00610         else if(current_herhalingen<=2200) {
00611             lcd.locate(0,0);
00612             lcd.printf("Vak %d akkoord?",doel_richting);
00613             lcd.locate(0,1);
00614             lcd.printf("Span aan");
00615         }//else if(current_herhalingen<=1600)
00616         else {
00617             lcd.locate(0,0);
00618             lcd.printf("Opnieuw richting");
00619             lcd.locate(0,1);
00620             lcd.printf("bepalen");
00621         }//else
00622     }//else if(state==METEN_RICHTING){
00623 
00624     else if(state==INSTELLEN_RICHTING) {
00625         lcd.cls();
00626         lcd.locate(0,0);
00627         lcd.printf("Instellen hoek");
00628         lcd.locate(0,1);
00629         lcd.printf("Even wachten...");
00630     }//else if(state==INSTELLEN_RICHTING){
00631 
00632     else if(state==SLAAN) {
00633         lcd.cls();
00634         lcd.locate(0,0);
00635         lcd.printf("Slaan, pas op");
00636         lcd.locate(0,1);
00637         lcd.printf("Let's pray");
00638     }//else if(state==INSTELLEN_RICHTING){
00639 
00640     else if(state==RETURN2RUST) {
00641         lcd.cls();
00642         lcd.locate(0,0);
00643         lcd.printf("Terug naar");
00644         lcd.locate(0,1);
00645         lcd.printf("0-positie");
00646     }//else if(state==INSTELLEN_RICHTING){
00647 
00648     else {
00649         lcd.cls();
00650         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
00651     }//else{
00652 }
00653 
00654 int main()
00655 {
00656     pwm_motor1.period_us(100);
00657     pwm_motor2.period_us(100);
00658     pc.baud(115200);
00659     statetimer.start();
00660     arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
00661     arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
00662     arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
00663     arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
00664     state = TEST;
00665     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
00666     //screen.attach(&screenupdate, 0.2);
00667     while(1) {
00668         screenupdate();
00669         wait(0.2);
00670     };
00671 }