Diff: main.cpp

Revision:

3:81a6009303a9

Parent:

2:3bf615031d7a

Child:

4:a0b0c944846e

--- a/main.cpp	Thu Oct 23 15:03:01 2014 +0000
+++ b/main.cpp	Mon Oct 27 14:31:43 2014 +0000
@@ -22,7 +22,7 @@
 #define M1_DIR PTA4 //kleine motor
 #define TSAMP 0.005  // Sampletijd, 200Hz
 
-TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2, NC, NC, TextLCD::HD44780); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x4); // rs, e, d4-d7 ok
+TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9); // rs, e, d4-d7 CONTROLEREN!! (Pinnen wel vrij :) )! //TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x4); // rs, e, d4-d7 ok
 
 Encoder motor2(PTD2,PTD0); //geel,wit kleine mtor
 Encoder motor1(PTD5,PTA13);//geel,wit
@@ -37,8 +37,8 @@
 MODSERIAL pc(USBTX,USBRX,64,1024);
 
 
-float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0; //variable to store value in for biceps 
-float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1; //variable to store value in for triceps 
+float emg0_value_f32,filtered_emg0_notch,filtered_emg0_notch_highpass,filtered_emg0_notch_highpass_lowpass,filtered_emg0_eindsignaal_abs,envelop_emg0,pwm_to_motor1,max_value_biceps; //variable to store value in for biceps 
+float emg1_value_f32,filtered_emg1_notch,filtered_emg1_notch_highpass,filtered_emg1_notch_highpass_lowpass,filtered_emg1_eindsignaal_abs,envelop_emg1,pwm_to_motor2,max_value_triceps; //variable to store value in for triceps 
 
 arm_biquad_casd_df1_inst_f32 notch_biceps;
 arm_biquad_casd_df1_inst_f32 notch_triceps;
@@ -67,9 +67,17 @@
 float envelop_biceps_states[4];
 float envelop_triceps_states[4];    
 
-enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN};
+enum slapstates {RUST,KALIBRATIE,RICHTEN,SLAAN}; //verschillende stadia definieren voor gebruik in CASES
 uint8_t state=RUST;
 
+volatile bool looptimerflag;
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+void keep_in_range(float * in, float min, float max); //keep in range
+
 void emgmeten(){
     /*put raw emg value in emg_value*/
     emg0_value_f32 = emg0.read();   
@@ -90,13 +98,14 @@
     arm_biquad_cascade_df1_f32(&envelop_triceps, &filtered_emg1_eindsignaal_abs, &envelop_emg1, 1 );
     }
 
-
 int main()
 {
     while(1) {
         pc.baud(38400); //PC baud rate is 38400 bits/seconde
         Ticker emg_timer;
         emg_timer.attach(emgmeten, TSAMP);
+        Ticker looptimer;
+        looptimer.attach(setlooptimerflag,TSAMP);
         arm_biquad_cascade_df1_init_f32(&notch_biceps,1 , notch_const, notch_biceps_states);
         arm_biquad_cascade_df1_init_f32(&highpass_biceps,1 ,highpass_const,highpass_biceps_states);
         arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 ,lowpass_const,lowpass_biceps_states);
@@ -114,31 +123,64 @@
             }                               
 
             case KALIBRATIE: {
-                lc.cls();
+                lcd.cls();
                 lcd.locate(0,0);         
                 lcd.printf("Kalibratie");   //regel 1 LCD scherm
                 lcd.locate(0,1);
-                lcd.printf("");             //regel 2 LCD scherm
-                wait(1);   
-                                              
+                lcd.printf("STAP 1: BICEPS");             //regel 2 LCD scherm
+                wait(1); 
+                max_value_biceps=0;
+                max_value_triceps=0;
+                for(int x=0; x<200; x++);{ //BICEPS METING
+                    if (envelop_emg0 > max_value_biceps); {
+                    max_value_biceps = envelop_emg0;
+                    }
+                    }
+                lcd.cls();
+                lcd.locate(0,0);         
+                lcd.printf("BICEPS OK");   //regel 1 LCD scherm
+                lcd.locate(0,1);
+                lcd.printf("STAP 2:TRICEPS");             //regel 2 LCD scherm   
+                wait(1);       
+                for(int x=0; x<200; x++); {//TRICEPS METING
+                    if (envelop_emg1 > max_value_triceps); {
+                    max_value_triceps = envelop_emg1;
+                    }
+                    } 
+                lcd.cls();
+                lcd.locate(0,0);         
+                lcd.printf("TRICEPS OK");   //regel 1 LCD scherm
+                lcd.locate(0,1);
+                lcd.printf("De waardes:");             //regel 2 LCD scherm 
+                wait(0.5);    
+                lcd.printf("%f,%f\n",max_value_biceps,max_value_triceps);                        
                 state = RICHTEN;                       
                 break;                          
             }                               
 
            case RICHTEN: {                   //Batje richten
-           lcd.printf("Richten");
-           wait(1);
-           state = SLAAN;
+                lcd.printf("Richten");
+                wait(1);
+                max_value = 0;
+                
+                    
+                keep_in_range(&pwm_to_motor, -1,1);
+                if(pwm_to_motor > 0)
+                    motordir1.write(1);
+                else
+                    motordir1.write(0);
+                pwm_motor1.write(abs(pwm_to_motor));      
+                state = SLAAN;
            break; 
             }                              
            case SLAAN: {                    //Balletje slaan
-           lcd.printf("Slaan!");
-           wait(1);
-           state = RICHTEN;
+                lcd.printf("Slaan!");
+                wait(1);
+                state = RICHTEN;
            break; 
             }  
 
-            default: {
+           default: {
                 state = RUST;
             }