Diff: main.cpp

Revision:

44:969348be74a5

Parent:

42:7164ccd2aa14

Child:

45:d0e9f586cd03

--- a/main.cpp	Tue Nov 01 10:30:56 2016 +0000
+++ b/main.cpp	Tue Nov 01 15:40:43 2016 +0000
@@ -9,12 +9,15 @@
 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++
 
-InterruptIn   button_calibration_biceps (SW3);                //button to start calibration biceps
-InterruptIn   button_calibration_triceps (SW2);               // button to start calibration tricps
+DigitalIn   button_calibration_biceps  (SW3);                //button to start calibration biceps
+DigitalIn   button_calibration_triceps (SW2);               // button to start calibration tricps
 
 Ticker      sample_timer;           //ticker
 Ticker      switch_function;         //ticker
+Ticker      ticker_calibration_biceps;
+Ticker      ticker_calibration_triceps;
 HIDScope    scope(5);               //open 3 channels in hidscope
+
 MODSERIAL pc(USBTX, USBRX);            //pc connection
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
@@ -31,8 +34,9 @@
 int    switch_signal_triceps=0;             // switching between motors. 
 volatile double cut_off_value_biceps_right =0.04;              //gespecificeerd door floortje
 volatile double cut_off_value_biceps_left  =-0.04;
-volatile double cut_off_value_triceps=0.04;             //gespecificeerd door floortje
+volatile double cut_off_value_triceps=-0.04;             //gespecificeerd door floortje
 double signal_biceps_sum;
+double bicepstriceps_rightarm;
 int motorswitch= 0; //start van de teller wordt op nul gesteld
 
 //variables and constants for calibration
@@ -112,12 +116,13 @@
 
 //callibration
 void calibration_biceps(){
+        if (button_calibration_biceps==0){
         pc.printf("start of calibration biceps, contract maximal \n");
         red=1;
         green=1;
         blue=0;
         
-        for(int n =0; n<3000;n++)                                                  //read for 2000 samples as calibration
+        for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
                 {
         emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
         emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
@@ -140,7 +145,7 @@
             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.5f);
+            wait(0.2f);
               
             if (motorswitch%2==0)
             {green=0;
@@ -148,17 +153,18 @@
             
             else       {green=1;
             red=0;}
-            
+                }
                 }
                 
 void calibration_triceps(){
+        if(button_calibration_triceps==0){
         red=1;
         green=1;
         blue=0;
       
         pc.printf("start of calibration triceps\r\n");
 
-        for(int n =0; n<3000;n++)                                                  //read for 2000 samples as calibration
+        for(int n =0; n<1500;n++)                                                  //read for 2000 samples as calibration
                 {
         emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
         emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
@@ -173,18 +179,18 @@
                 }
                 wait(0.001f); //to sample at same freq; 1000Hz
                 }
-            cut_off_value_triceps=percentage_max_triceps*max_triceps; 
+            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.5f);
+            wait(0.2f);
             if (motorswitch%2==0)
             {green=0;
             red=1;}
             
             else       {green=1;
             red=0;}  
-            
+                }
                 }
 
 void filter(){
@@ -192,7 +198,7 @@
         emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
         emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
         emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
-                emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
+        emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
         emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
         
         //triceps right arm read+filtering
@@ -211,7 +217,7 @@
           
         //signal substraction of filter biceps and triceps. right Biceps + left biceps -
         signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
-               
+        bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right;       
         //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
         if (signal_biceps_sum>cut_off_value_biceps_right)
         {onoffsignal_biceps=1;}
@@ -226,7 +232,7 @@
                       
         //creating on/off signal for switch (left arm)
         
-        if (emg_filtered_triceps_right>cut_off_value_triceps)
+        if (bicepstriceps_rightarm<cut_off_value_triceps)
         {
         switch_signal_triceps=1;    
         }    
@@ -240,7 +246,7 @@
         scope.set(0, emg_filtered_biceps_right);            //set emg signal to scope in channel 0
         scope.set(1, emg_filtered_triceps_right);           // set emg signal to scope in channel 1
         scope.set(2, emg_filtered_biceps_left);                     // set emg signal to scope in channel 2
-   
+        scope.set(3, onoffsignal_biceps);
         
         scope.send();                       //send all the signals to the scope
                 }
@@ -253,16 +259,16 @@
 
 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
 switch_function.attach(&SwitchN,1.0);                   //switch is every second available
-button_calibration_biceps.fall (&calibration_biceps);   //to call calibration biceps, stop everything else
-button_calibration_triceps.fall(&calibration_triceps);  //to call calibration triceps, stop everything else
+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
 
-  if (motorswitch%2==0)  
-      { pc.printf("If you contract the right arm, the robot will go right \r\n");
+  if (motorswitch%2==0) {
+        pc.printf("If you contract the right arm, the robot will go right \r\n");
         pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
         pc.printf("\r\n");
-    green=0;
-    red=1;
-    blue=1;
+        green=0;
+        red=1;
+        blue=1;
           }
     
     else
@@ -285,8 +291,8 @@
          if (motorswitch%2==0)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
          { 
            richting_motor1 = 1;
-           pwm_motor1 = 1; 
-          
+           pwm_motor1 = 0.5; 
+           pc.printf("ccw m1\r\n");
            
                   
          } 
@@ -304,7 +310,8 @@
          if (motorswitch%2==0)                     // als d is ingedrukt en n is even dan gebeurd het volgende
          {
            richting_motor1 = 0;
-           pwm_motor1 = 1;
+           pwm_motor1 = 0.5;
+           pc.printf("cw 1 aan\r\n");
            
            
         } 
@@ -323,5 +330,4 @@
        }              
                
 }
-        
-}
\ No newline at end of file
+ }       
\ No newline at end of file