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