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 (2000)
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 float snelheid;
00107 
00108 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES
00109 state_t state = RUST;
00110 
00111 void rust()
00112 {
00113     current_herhalingen = previous_herhalingen + 1;
00114     previous_herhalingen = current_herhalingen;
00115 }//void rust
00116 
00117 void pieken_tellen()
00118 {
00119     if (emg_filtered>=emg_treshhold_hoog) {
00120         aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
00121     }//if
00122     if (aanspan==true && emg_filtered<=emg_treshhold_laag) { //== ipv =, anders wordt aanspan true gemaakt
00123         aanspan=false;
00124         aantal_pieken++;
00125         doel = aantal_pieken-(((aantal_pieken-1)/3)*3); //aantal_pieken-((aantal_pieken/3)*3)+1;
00126 
00127     }//if
00128 }//void pieken_tellen
00129 
00130 void emg_filtering()
00131 {
00132     uint16_t emg_value;
00133     float emg_value_f32;
00134     emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
00135     emg_value_f32 = emg.read();
00136 
00137     arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
00138     arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
00139     arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
00140     emg_filtered = fabs(emg_filtered);
00141     arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
00142     //scope.set(0,emg_value);     //uint value
00143     //scope.set(1,emg_filtered);  //processed float
00144     if(state!=EMG_KALIBRATIE) {
00145         pieken_tellen();
00146     }//if
00147     //pc.printf("%d\n\r",doel);
00148     scope.set(0, doel);
00149     scope.set(1, aantal_pieken);
00150     scope.send();
00151 }//void emg_filtering()
00152 
00153 void emg_max_meting()
00154 {
00155     emg_filtering();
00156     if (emg_filtered>=emg_max) {
00157         emg_max=emg_filtered;
00158     }//if
00159     emg_treshhold_laag = 0.4*emg_max;
00160     emg_treshhold_hoog = 0.7*emg_max;
00161 }//void emg_max_meting
00162 
00163 void akkoord_geven()
00164 {
00165     emg_filtering();
00166 }
00167 
00168 void emg_kalibratie()
00169 {
00170     //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.
00171     current_herhalingen = previous_herhalingen + 1;
00172     previous_herhalingen = current_herhalingen;
00173     //}
00174     if(current_herhalingen<=1000) {
00175         emg_max_meting();
00176     }//if
00177 }//void emg_kalibratie
00178 
00179 void arm_kalibratie()
00180 {
00181     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
00182     motor1.setPosition(0);
00183     //motor2.setPosition(0);
00184     akkoord_geven();
00185 }//void arm_kalibratie
00186 
00187 void doel_bepaling()
00188 {
00189     if(200<=current_herhalingen && current_herhalingen <1200) {
00190         emg_filtering();
00191         doel = aantal_pieken-(((aantal_pieken-1)/3)*3);
00192     }//if
00193     else if(current_herhalingen == 1200 && state==METEN_HOOGTE) {
00194         doel_hoogte = doel;
00195         aantal_pieken = 0;
00196         doel = 0;
00197     } else if(current_herhalingen == 1200 && state==METEN_RICHTING) {
00198         doel_richting = doel;
00199         aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
00200         doel = 0;
00201     } else if(1200<current_herhalingen && current_herhalingen<=2200) {
00202         akkoord_geven();
00203     }//else if
00204     else {
00205     }//else
00206 }//void doel_bepaling
00207 
00208 void meten_hoogte()
00209 {
00210     current_herhalingen = previous_herhalingen + 1;
00211     previous_herhalingen = current_herhalingen;
00212     doel_bepaling();
00213 }//void meten_hoogte
00214 
00215 void meten_richting()
00216 {
00217     current_herhalingen = previous_herhalingen + 1;
00218     previous_herhalingen = current_herhalingen;
00219     doel_bepaling();
00220 }//void meten_richting
00221 
00222 void GotoPosition (float position_setpoint_rad, float speed_radpersecond)
00223 {
00224     static float setpoint = 0; 
00225     if (setpoint < position_setpoint_rad) {
00226         setpoint += (0.005 * speed_radpersecond);
00227         if (setpoint > position_setpoint_rad) {
00228             setpoint = position_setpoint_rad;
00229         }
00230     }
00231 
00232     else if (setpoint > position_setpoint_rad) {
00233         setpoint -= (0.005 * speed_radpersecond);
00234         if (setpoint < position_setpoint_rad) {
00235             setpoint = position_setpoint_rad;
00236         }
00237     } else if (setpoint == position_setpoint_rad) {
00238         current_herhalingen = previous_herhalingen + 1;
00239         previous_herhalingen = current_herhalingen;
00240     }
00241     
00242     pc.printf("%f\r\n", setpoint); 
00243     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00244     pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
00245     PWM1_percentage = pid(setpoint, 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)
00280 {
00281     static float setpoint = 0; 
00282     if (setpoint < position2_setpoint_rad) {
00283         setpoint += (0.005 * 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 -= (0.005 * 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     pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa
00301     PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad);
00302 
00303     if (PWM2_percentage < -100) {
00304         PWM2_percentage = -100;
00305     } else if (PWM2_percentage >100) {
00306         PWM2_percentage =100;
00307     }
00308 
00309     if(PWM2_percentage < 0) {
00310         motordir2 = 1;
00311     }//if
00312     else {
00313         motordir2 = 0;
00314     }//else
00315 
00316     pwm_motor2.write(abs(PWM2_percentage/100.));
00317     prev_setpoint = setpoint;
00318 }
00319 
00320 float pid_motor2(float setpoint, float measurement)
00321 {
00322     float error;
00323     static float prev_error = 0;
00324     float           out_p = 0;
00325     float           out_d = 0;
00326     error  = (setpoint-measurement);
00327     out_p  = error*K_P_motor2;
00328     out_d  = (error-prev_error)*K_D_motor2;
00329     prev_error = error;
00330     return out_p + out_d;
00331 }//float pid
00332 
00333 void clamp(float* in, float min, float max)
00334 {
00335 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min;
00336 }//void clamp
00337 
00338 void statemachinefunction()
00339 {
00340     statetimer.reset();
00341     //pc.printf(".");
00342     switch(state) {
00343         case RUST: {
00344             rust();
00345             /*voorwaarde wanneer hij door kan naar de volgende case*/
00346             if (current_herhalingen == 100 && EMG == 1) {
00347                 current_herhalingen = 0;
00348                 previous_herhalingen = 0;
00349                 state = EMG_KALIBRATIE;
00350                 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
00351             }//if (current_herhalingen == 100 && EMG == 1)
00352             else if(current_herhalingen == 100) {
00353                 current_herhalingen = 0;
00354                 previous_herhalingen = 0;
00355                 state = ARM_KALIBRATIE;
00356             }//else
00357             break;
00358         }//case RUST:
00359 
00360         case EMG_KALIBRATIE: {
00361             emg_kalibratie();
00362             if (current_herhalingen >=1000) { /*waarom >= en niet ==?*/
00363                 current_herhalingen = 0;
00364                 previous_herhalingen = 0;
00365                 aantal_pieken = 0;
00366                 doel = 0;
00367                 state = ARM_KALIBRATIE;
00368             }//if (current_herhalingen >=1000)
00369             break;
00370         }//case EMG_KALIBRATIE
00371 
00372         case ARM_KALIBRATIE: {
00373             arm_kalibratie();
00374             if (aantal_pieken == 1) {
00375                 current_herhalingen = 0;
00376                 previous_herhalingen = 0;
00377                 aantal_pieken = 0;
00378                 doel = 0;
00379                 state = METEN_HOOGTE;
00380             }//if (current_herhalingen == 100)
00381             break;
00382         }//case ARM_KALIBRATIE:
00383 
00384         case METEN_HOOGTE: {
00385             meten_hoogte();
00386             if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_hoogte>=1 ) {
00387                 current_herhalingen = 0;
00388                 previous_herhalingen = 0;
00389                 aantal_pieken = 0;
00390                 doel = 0;
00391                 //doel_hoogte = 0;
00392                 state = METEN_RICHTING;
00393             }//if (current_herhalingen == 2800 && aantal_pieken == 1)
00394             else if (current_herhalingen == 2200) {
00395                 current_herhalingen = 0;
00396                 previous_herhalingen = 0;
00397                 aantal_pieken = 0;
00398                 doel = 0;
00399                 state = METEN_HOOGTE;
00400             }///else
00401             break;
00402         }//case METEN_HOOGTE
00403 
00404         case METEN_RICHTING: {
00405             meten_richting();
00406             if (1200 < current_herhalingen && current_herhalingen <2200 && aantal_pieken == 1 && doel_richting>=1 ) {
00407                 current_herhalingen = 0;
00408                 previous_herhalingen = 0;
00409                 aantal_pieken = 0;
00410                 doel = 0;
00411                 state = INSTELLEN_RICHTING;
00412             }//if (current_herhalingen == 2800 && aantal_pieken == 1)
00413             else if (current_herhalingen == 2200) {
00414                 current_herhalingen = 0;
00415                 previous_herhalingen = 0;
00416                 aantal_pieken = 0;
00417                 doel = 0;
00418                 state = METEN_RICHTING;
00419             }///else
00420             break;
00421         }//case METEN_RICHTING
00422 
00423 
00424         case TEST: {
00425             state = RUST;
00426             break;
00427         }
00428 
00429         case INSTELLEN_RICHTING: {
00430             //instellen_richting();
00431             if (doel_richting == 1) {
00432                 position2_setpoint = 0;
00433             } else if (doel_richting ==2) {
00434                 position2_setpoint = 14.5;
00435             } else {
00436                 position2_setpoint = 25;
00437             }
00438 
00439             translatie(position2_setpoint, 1);
00440 
00441             if (current_herhalingen >= 400) {
00442                 current_herhalingen = 0;
00443                 previous_herhalingen = 0;
00444                 doel_richting = 0; 
00445                 state = SLAAN;
00446             }//if (current_herhalingen == 100
00447             break;
00448         }//case INSTELLEN_RICHTING
00449 
00450         case SLAAN: {
00451             if (doel_hoogte == 1) {
00452                 snelheid = 2;
00453             } else if (doel_hoogte ==2) {
00454                 snelheid = 2.25;
00455             } else {
00456                 snelheid = 5;
00457             }
00458             GotoPosition(1.9, snelheid);
00459             if (current_herhalingen == 400) {
00460                 current_herhalingen = 0;
00461                 previous_herhalingen = 0;
00462                 prev_setpoint =0;
00463                 //setpoint = 0;
00464                 state = RETURN2RUST;
00465             }//if (current_herhalingen == 100)
00466             break;
00467         }//case SLAAN
00468 
00469         case RETURN2RUST: {
00470             GotoPosition(0,0.1);
00471             doel_richting = 0;
00472             doel_hoogte = 0;
00473             if (current_herhalingen >= 200) {
00474                 current_herhalingen = 0; 
00475                 translatie(0,1.6);
00476                 if (current_herhalingen >=200){
00477                 state = RUST;
00478                 current_herhalingen = 0;
00479                 previous_herhalingen = 0;
00480                 current_herhalingen = 0;
00481                 current_herhalingen = 0;
00482                 }
00483             }//if (current_herhalingen == 100)
00484             
00485             break;
00486         }// case RETURN2RUST
00487 
00488         default: {
00489             state = RUST;
00490         }//default
00491 
00492     }//switch(state)
00493 }//void statemachinefunction
00494 
00495 
00496 void screenupdate()
00497 {
00498     if(state==RUST) {
00499         lcd.cls();
00500         lcd.locate(0,0);
00501         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
00502         lcd.locate(0,1);
00503         lcd.printf("  GROEP 7   ");
00504     }//if(state==RUST)
00505 
00506     else if(state==EMG_KALIBRATIE) {
00507         lcd.cls();
00508         lcd.locate(0,0);
00509         lcd.printf("Max. aanspannen");
00510         if(current_herhalingen<=200) {
00511             lcd.locate(0,1);
00512             lcd.printf("nog 5 sec.");
00513         }//if(current_herhalingen<=200)
00514         else if(current_herhalingen<=400) {
00515             lcd.locate(0,1);
00516             lcd.printf("nog 4 sec.");
00517         }//else if(current_herhalingen<=400)
00518         else if(current_herhalingen<=600) {
00519             lcd.locate(0,1);
00520             lcd.printf("nog 3 sec.");
00521         }//else if(current_herhalingen<=600)
00522         else if(current_herhalingen<=800) {
00523             lcd.locate(0,1);
00524             lcd.printf("nog 2 sec.");
00525         }//else if(current_herhalingen<=800)
00526         else if(current_herhalingen<=1000) {
00527             lcd.locate(0,1);
00528             lcd.printf("nog 1 sec.");
00529         }//else if(current_herhalingen<=1000)
00530     }//else if(state==EMG_KALIBRATIE)
00531 
00532     else if(state==ARM_KALIBRATIE) {
00533         lcd.cls();
00534         lcd.locate(0,0);
00535         lcd.printf("Set arm to zero");
00536         lcd.locate(0,1);
00537         lcd.printf("Klaar? Span aan");
00538     }//else if(state==ARM_KALIBRATIE)
00539 
00540     else if(state==METEN_HOOGTE) {
00541         lcd.cls();
00542         if(current_herhalingen<=200) {
00543             lcd.locate(0,0);
00544             lcd.printf("Hoogte bepalen:");
00545             lcd.locate(0,1);
00546             lcd.printf("span aan per vak");
00547         }//if(current_herhalingen<=200){
00548         else if(200<=current_herhalingen && current_herhalingen<1200) {
00549             lcd.locate(0,0);
00550             lcd.printf("Vak %d",doel);
00551             if(current_herhalingen<=400) {
00552                 lcd.locate(0,1);
00553                 lcd.printf("nog 5 sec.");
00554             }//if(current_herhalingen<=400)
00555             else if(current_herhalingen<=600) {
00556                 lcd.locate(0,1);
00557                 lcd.printf("nog 4 sec.");
00558             }//else if(current_herhalingen<=600)
00559             else if(current_herhalingen<=800) {
00560                 lcd.locate(0,1);
00561                 lcd.printf("nog 3 sec.");
00562             }//else if(current_herhalingen<=800)
00563             else if(current_herhalingen<=1000) {
00564                 lcd.locate(0,1);
00565                 lcd.printf("nog 2 sec.");
00566             }//else if(current_herhalingen<=1000)
00567             else if(current_herhalingen<1200) {
00568                 lcd.locate(0,1);
00569                 lcd.printf("nog 1 sec.");
00570             }//else if(current_herhalingen<=1200)
00571         }//else if(200<=current_herhalingen<=1200)
00572         else if(current_herhalingen<=2200) {
00573             lcd.locate(0,0);
00574             lcd.printf("Vak %d akkoord?",doel_hoogte);
00575             lcd.locate(0,1);
00576             lcd.printf("Span aan");
00577         }//else if(current_herhalingen<=1600){
00578         else {
00579             lcd.locate(0,0);
00580             lcd.printf("Opnieuw hoogte");
00581             lcd.locate(0,1);
00582             lcd.printf("bepalen");
00583         }//else{
00584     }//else if(state==METEN_HOOGTE){
00585 
00586     else if(state==METEN_RICHTING) {
00587         lcd.cls();
00588         if(current_herhalingen<=200) {
00589             lcd.locate(0,0);
00590             lcd.printf("Richting bepalen:");
00591             lcd.locate(0,1);
00592             lcd.printf("span aan per vak");
00593         }//if(current_herhalingen<=200)
00594         else if(200<=current_herhalingen && current_herhalingen<1200) {
00595             lcd.locate(0,0);
00596             lcd.printf("Vak %d",doel);
00597             if(current_herhalingen<=400) {
00598                 lcd.locate(0,1);
00599                 lcd.printf("nog 5 sec.");
00600             }//if(current_herhalingen<=400)
00601             else if(current_herhalingen<=600) {
00602                 lcd.locate(0,1);
00603                 lcd.printf("nog 4 sec.");
00604             }//else if(current_herhalingen<=600)
00605             else if(current_herhalingen<=800) {
00606                 lcd.locate(0,1);
00607                 lcd.printf("nog 3 sec.");
00608             }//else if(current_herhalingen<=800)
00609             else if(current_herhalingen<=1000) {
00610                 lcd.locate(0,1);
00611                 lcd.printf("nog 2 sec.");
00612             }//else if(current_herhalingen<=1000)
00613             else if(current_herhalingen<1200) {
00614                 lcd.locate(0,1);
00615                 lcd.printf("nog 1 sec.");
00616             }//else if(current_herhalingen<=1200)
00617         }//else if(200<=current_herhalingen<=1200)
00618         else if(current_herhalingen<=2200) {
00619             lcd.locate(0,0);
00620             lcd.printf("Vak %d akkoord?",doel_richting);
00621             lcd.locate(0,1);
00622             lcd.printf("Span aan");
00623         }//else if(current_herhalingen<=1600)
00624         else {
00625             lcd.locate(0,0);
00626             lcd.printf("Opnieuw richting");
00627             lcd.locate(0,1);
00628             lcd.printf("bepalen");
00629         }//else
00630     }//else if(state==METEN_RICHTING){
00631 
00632     else if(state==INSTELLEN_RICHTING) {
00633         lcd.cls();
00634         lcd.locate(0,0);
00635         lcd.printf("Instellen hoek");
00636         lcd.locate(0,1);
00637         lcd.printf("Even wachten...");
00638     }//else if(state==INSTELLEN_RICHTING){
00639 
00640     else if(state==SLAAN) {
00641         lcd.cls();
00642         lcd.locate(0,0);
00643         lcd.printf("Slaan, pas op");
00644         lcd.locate(0,1);
00645         lcd.printf("Let's pray");
00646     }//else if(state==INSTELLEN_RICHTING){
00647 
00648     else if(state==RETURN2RUST) {
00649         lcd.cls();
00650         lcd.locate(0,0);
00651         lcd.printf("Terug naar");
00652         lcd.locate(0,1);
00653         lcd.printf("0-positie");
00654     }//else if(state==INSTELLEN_RICHTING){
00655 
00656     else {
00657         lcd.cls();
00658         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
00659     }//else{
00660 }
00661 
00662 int main()
00663 {
00664     pwm_motor1.period_us(100);
00665     pwm_motor2.period_us(100);
00666     pc.baud(115200);
00667     statetimer.start();
00668     arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
00669     arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
00670     arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
00671     arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
00672     state = TEST;
00673     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
00674     //screen.attach(&screenupdate, 0.2);
00675     while(1) {
00676         screenupdate();
00677         wait(0.2);
00678     };
00679 }