Revision 35:7d9fca0b1545, committed 2015-10-22

Comitter:

Vigilance88

Date:

Thu Oct 22 14:18:41 2015 +0000

Parent:

34:d6ec7c634763

Child:

36:4d4fc200171b

Commit message:

added minimum emg measurement to calibration

Changed in this revision

--- a/main.cpp	Thu Oct 22 12:59:55 2015 +0000
+++ b/main.cpp	Thu Oct 22 14:18:41 2015 +0000
@@ -84,10 +84,10 @@
 --------------------------------------------------------------------------------------------------------------------*/
 
 //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction
-double emg_biceps; double biceps_power; double bicepsMVC = 0;
-double emg_triceps; double triceps_power; double tricepsMVC = 0;
-double emg_flexor; double flexor_power; double flexorMVC = 0;
-double emg_extens; double extens_power; double extensMVC = 0;
+double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin;
+double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin;
+double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin;
+double emg_extens; double extens_power; double extensMVC = 0; double extensmin;
 
 int muscle;             //Muscle selector for MVC measurement
 double calibrate_time;  //Elapsed time for each MVC measurement
@@ -95,10 +95,10 @@
 //PID variables
 double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
-const double m1_kp=0.001; const double m1_ki=0.0125; const double m1_kd=0.1;   //Proportional, integral and derivative gains.
+const double m1_kp=0.1; const double m1_ki=0.0125; const double m1_kd=0.02;   //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
+const double m2_kp=0.1; const double m2_ki=0.0125; const double m2_kd=0.02;   //Proportional, integral and derivative gains.
 
 //Calibration, checks if elbow/shoulder are done
 bool done1 = false;
@@ -212,7 +212,9 @@
 void start_sampling(void);  //Attaches the sample_filter function to a 500Hz ticker
 void stop_sampling(void);   //Stops sample_filter
 void start_control(void);   //Attaches the control function to a 100Hz ticker
+void start_dlscontrol(void);
 void stop_control(void);    //Stops control function
+void emg_min();
 
 //Keeps the input between min and max value
 void keep_in_range(double * in, double min, double max);
@@ -317,9 +319,9 @@
                 
                 }//end while
                 break;
-            case 's':
-            case 'S':
-                pc.printf("=> You chose control \r\n\n");
+            case 't':
+            case 'T':
+                pc.printf("=> You chose TRIG control \r\n\n");
                 wait(1);
                 start_sampling();
                 wait(1);
@@ -327,6 +329,16 @@
                 wait(1);   
                 controlButtons();
                 break;
+            case 'd':
+            case 'D':
+                pc.printf("=> You chose DLS control \r\n\n");
+                wait(1);
+                start_sampling();
+                wait(1);
+                start_dlscontrol();
+                wait(1);   
+                controlButtons();
+                break;    
             case 'R':
                 red=!red;
                 pc.printf("=> Red LED triggered \n\r");
@@ -414,7 +426,7 @@
     pc.printf("Reference position: %f and %f \r\n",x,y);
     pc.printf("Current position: %f and %f \r\n",current_x,current_y);
     pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
-    pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
+    pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
     pc.printf("PID output: %f and %f \r\n",u1,u2);
     pc.printf("----------------------------------------\r\n\n");
     }
@@ -586,6 +598,29 @@
    
 }
 
+void emg_min()
+{  
+            
+            if(biceps_power<bicepsmin){
+            bicepsmin=biceps_power;
+            }    
+            
+            if(triceps_power<tricepsmin){
+            tricepsmin=triceps_power;
+            }            
+
+            if(flexor_power<flexormin){
+            flexormin=flexor_power;
+            }            
+ 
+            if(extens_power < extensmin){   
+            extensmin = extens_power;
+            }    
+        
+    calibrate_time = calibrate_time + 0.002;
+    
+}
+
 //EMG calibration
 void calibrate_emg()
 {
@@ -593,6 +628,27 @@
    
    pc.printf("Testcode calibration \r\n");
    wait(1);
+   pc.printf("Starting minimum measurement, relax all muscles.");
+   wait(1);
+   pc.printf(" Press any key to begin... "); wait(1);
+   char input;
+   input=pc.getc();
+   pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
+   
+   start_sampling();
+   timer.attach(&emg_min,SAMPLE_RATE);
+   wait(5);
+   timer.detach();
+   pc.printf("\r\n Measurement complete."); wait(1);
+   pc.printf("\r\n Biceps min = %f \r\n",bicepsmin); wait(1);
+   pc.printf("\r\n Triceps min = %f \r\n",tricepsmin); wait(1);
+   pc.printf("\r\n Flexor min = %f \r\n",flexormin); wait(1);
+   pc.printf("\r\n Extensor min = %f \r\n",extensmin); wait(1);
+   
+   calibrate_time=0;
+      pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(1); 
    pc.printf("+ means current sample is higher than stored MVC\r\n");
    pc.printf("- means current sample is lower than stored MVC\r\n");
    wait(2);
@@ -601,7 +657,6 @@
    pc.printf("----------------\r\n "); 
    wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
-   char input;
    input=pc.getc();
    pc.putc(input);
    pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
@@ -634,7 +689,6 @@
    pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
    pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
    start_sampling();
-   muscle=1;
    timer.attach(&emg_mvc,0.002);
    wait(5);
    timer.detach();
@@ -646,9 +700,50 @@
    
    //Flexor:
    muscle=3;
+   pc.printf("\r\n----------------\r\n "); 
+   pc.printf(" Flexor is next.\r\n "); 
+   pc.printf("----------------\r\n "); 
+   wait(1);
+
+   pc.printf(" Press any key to begin... "); wait(1);
+   input=pc.getc();
+   pc.putc(input);
+   pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
+   start_sampling();
+   timer.attach(&emg_mvc,0.002);
+   wait(5);
+   timer.detach();
+   pc.printf("\r\n Flexor MVC = %f \r\n",flexorMVC);
+   
+   pc.printf("Calibrate_emg() exited \r\n");
+   pc.printf("Measured time: %f seconds \r\n",calibrate_time);
+   calibrate_time=0;
+   
    //Extensor:
+   
    muscle=4;
+   pc.printf("\r\n----------------\r\n "); 
+   pc.printf(" Extensor is next.\r\n "); 
+   pc.printf("----------------\r\n "); 
+   wait(1);
+
+   pc.printf(" Press any key to begin... "); wait(1);
+   input=pc.getc();
+   pc.putc(input);
+   pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
+   pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
+   start_sampling();
+   timer.attach(&emg_mvc,0.002);
+   wait(5);
+   timer.detach();
+   pc.printf("\r\n Extensor MVC = %f \r\n",extensMVC);
    
+   pc.printf("Calibrate_emg() exited \r\n");
+   pc.printf("Measured time: %f seconds \r\n",calibrate_time);
+   calibrate_time=0;
    //Stop sampling, detach ticker
    stop_sampling();
    
@@ -675,12 +770,13 @@
    /* 
    
     //normalize emg to value between 0-1
-    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
-    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
-    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
-    biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin)
+    norm_biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin)
+    norm_triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin)
+    norm_flexor = (flexor_power - flexormin) / (flexorMVC - flexormin)
+    norm_extens = (extens_power - extensmin) / (extensMVC - extensmin)
     
     //threshold detection! buffer?
+    TODO
     
     //analyze emg (= velocity)
     if (biceps>triceps && biceps > 0.1)
@@ -972,7 +1068,8 @@
    //endbox
    wait(1);
    pc.printf("[C]alibration\r\n"); wait(0.2);
-   pc.printf("[S]tart Control with buttons\r\n"); wait(0.2);
+   pc.printf("[T]RIG Control with buttons\r\n"); wait(0.2);
+   pc.printf("[D]LS Control with buttons\r\n"); wait(0.2);
    pc.printf("[Q]uit Robot Program\r\n"); wait(0.2);
    pc.printf("[R]ed LED\r\n"); wait(0.2); 
    pc.printf("[G]reen LED\r\n"); wait(0.2);
@@ -1003,6 +1100,15 @@
 //Start control
 void start_control(void)
 {
+    control_timer.attach(&control,CONTROL_RATE);     //100 Hz control
+    
+    //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
+    blue=0;
+    pc.printf("||- started control -|| \r\n");
+}
+
+void start_dlscontrol(void)
+{
     control_timer.attach(&dlscontrol,CONTROL_RATE);     //100 Hz control
     
     //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
@@ -1010,6 +1116,7 @@
     pc.printf("||- started control -|| \r\n");
 }
 
+
 //stop control
 void stop_control(void)
 {