Diff: main.cpp

Revision:

48:0a16643c9de4

Parent:

47:ddaa59d48aca

diff -r ddaa59d48aca -r 0a16643c9de4 main.cpp
--- a/main.cpp	Thu Nov 03 08:43:08 2016 +0000
+++ b/main.cpp	Thu Nov 03 08:49:36 2016 +0000
@@ -12,13 +12,21 @@
 AnalogIn    emg_biceps_right_in( A0 );              //analog in to get EMG biceps  (r) in to c++
 AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
 AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++
-Ticker      sample_timer;               //ticker
-Ticker      ticker_switch;              //ticker
+
 HIDScope    scope(5);                   //open 5 channels in hidscope
 MODSERIAL pc(USBTX, USBRX);             //pc connection
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
+DigitalOut blue(LED_BLUE);
 
+    DigitalIn   button_calibration_biceps  (SW3);                //button to start calibration biceps
+    DigitalIn   button_calibration_triceps (SW2);               // button to start calibration tricps
+    
+//tickers
+Ticker      sample_timer;               //ticker
+Ticker      ticker_switch;              //ticker
+Ticker      ticker_calibration_biceps;
+Ticker      ticker_calibration_triceps;
 //motors
 DigitalOut richting_motor1(D7); ///motor 1 aansluiting op motor 1
 PwmOut pwm_motor1(D6);
@@ -40,6 +48,12 @@
 float speedmotor1=0.18;
 float speedmotor2=1.0;
 
+//calibration
+const float percentage_max_triceps=0.25;
+const float percentage_max_biceps =0.3;
+double max_biceps;                          //calibration maximum biceps
+double max_triceps;                         //calibration maximum triceps
+
 int cw=0;
 int ccw=1;
  
@@ -152,6 +166,89 @@
         
         scope.send();                       //send all the signals to the scope
                 }
+ //============================================================================================================
+void calibration_biceps(){
+        if (button_calibration_biceps==0){
+        ticker_switch.detach();
+        pc.printf("start of calibration biceps, contract maximal \n");
+        red=1;
+        green=1;
+        blue=0;
+        
+        for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
+                {
+        double emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
+        double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
+        double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
+        double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
+        double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
+                                
+            if (emg_filtered_biceps_right > max_biceps)                    //determine what the highest reachable emg signal is
+                {
+            max_biceps = emg_filtered_biceps_right;
+            
+                }
+                wait(0.001f); //to sample at same freq; 1000Hz
+                }
+            cut_off_value_biceps_right=percentage_max_biceps*max_biceps; 
+            cut_off_value_biceps_left=-cut_off_value_biceps_right;
+            //toggle lights
+            blue=!blue;
+            
+            pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );   
+            pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
+            
+            wait(0.2f);
+              
+            if (switch_signal%2==0)
+            {green=0;
+            red=1;}
+            
+            else       {green=1;
+            red=0;}
+                }
+            ticker_switch.attach(&switch_function,1.0);
+                }
+  //=======================================================================              
+void calibration_triceps(){
+        if(button_calibration_triceps==0){
+        ticker_switch.detach();
+        red=1;
+        green=1;
+        blue=0;
+      
+        pc.printf("start of calibration triceps\r\n");
+
+        for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
+                {
+        double emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
+        double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
+        double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
+        double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
+        double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
+                              
+            if (emg_filtered_triceps_right > max_triceps)                    //determine what the highest reachable emg signal is
+                {
+            max_triceps = emg_filtered_triceps_right;
+            
+                }
+                wait(0.001f); //to sample at same freq; 1000Hz
+                }
+            cut_off_value_triceps=-percentage_max_triceps*max_triceps; 
+            pc.printf(" end of calibration\r\n");   
+            pc.printf(" change of cv triceps: %f ",cut_off_value_triceps ); 
+            blue=!blue;
+            wait(0.2f);
+            if (switch_signal%2==0)
+            {green=0;
+            red=1;}
+            
+            else       {green=1;
+            red=0;}  
+                }
+            ticker_switch.attach(&switch_function,1.0);    
+                }
+  //=======================================================================                             
 
 //program
 
@@ -160,9 +257,12 @@
 pc.baud(115200);
 green=1;
 red=0;
+blue=1;
 
 sample_timer.attach(&filter, 0.001);        //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
 ticker_switch.attach(&switch_function,1.0);
+ticker_calibration_biceps.attach (&calibration_biceps,2.0);   //to call calibration biceps, stop everything else
+ticker_calibration_triceps.attach(&calibration_triceps,2.0);  //to call calibration triceps, stop everything else
 
 pc.printf("We will start the demonstration\r\n");