Revision 39:e77f844d10d9, committed 2015-10-23

Comitter:

Vigilance88

Date:

Fri Oct 23 00:36:37 2015 +0000

Parent:

38:c8ac615d0c8f

Child:

40:d62f96ed44c0

Commit message:

added EMG reference calculation - needs lots of testing

Changed in this revision

--- a/main.cpp	Fri Oct 23 00:02:19 2015 +0000
+++ b/main.cpp	Fri Oct 23 00:36:37 2015 +0000
@@ -89,9 +89,12 @@
 double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=0;
 double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=0;
 double emg_extens; double extens_power; double extensMVC = 0; double extensmin=0;
-//Normalized emg values
-double biceps, triceps, flexor, extens;
-
+//Normalize and compare variables
+double biceps, triceps, flexor, extens;         //Storage for normalized emg
+double xdir, ydir;                              //EMG reference position directions
+double xpower, ypower;                          //EMG reference magnitude
+double dx, dy;                                  //integration of EMG (velocity) to position
+        
 int muscle;             //Muscle selector for MVC measurement, 1 = first emg etc.
 double calibrate_time;  //Elapsed time for each MVC measurement
 
@@ -161,8 +164,9 @@
 //Reference position
 double x; double y;
 
-//Select whether to use Trig or DLS method
+//Select whether to use Trig or DLS method, emg true or false
 int control_method;
+bool emg_control;
 
 //Inverse Kinematics - Trig / Gonio method. 
 //First convert reference position to angle needed, then compare that angle to current angle.
@@ -325,10 +329,11 @@
                 break;
             case 't':
             case 'T':
-                pc.printf("=> You chose TRIG control \r\n\n");
+                pc.printf("=> You chose TRIG button control \r\n\n");
                 wait(1);
                 start_sampling();
                 wait(1);
+                emg_control=false;
                 control_method=1;
                 start_control();
                 wait(1);   
@@ -336,15 +341,28 @@
                 break;
             case 'd':
             case 'D':
-                pc.printf("=> You chose DLS control \r\n\n");
+                pc.printf("=> You chose DLS button control \r\n\n");
                 wait(1);
                 start_sampling();
                 wait(1);
+                emg_control=false;
                 control_method=2;
                 start_control();
                 wait(1);   
                 controlButtons();
                 break;    
+            case 'e':
+            case 'E':
+                pc.printf("=> You chose EMG DLS control \r\n\n");
+                wait(1);
+                start_sampling();
+                wait(1);
+                emg_control=true;
+                control_method=2;
+                start_control();
+                wait(1);   
+                controlButtons();
+                break;        
             case 'R':
                 red=!red;
                 pc.printf("=> Red LED triggered \n\r");
@@ -781,13 +799,22 @@
 
 //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
 void control()
-{
-     
+{ 
+    if(emg_control=true){
+    //TODO some idle time with static reference before EMG kicks in
+    //emg_control_time += CONTROL_RATE; 
+    // if emg_control_time < 5 seconds, reference is  x=0; y=0;    
     //normalize emg to value between 0-1
     biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
     triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
     flexor = (flexor_power - flexormin) / (flexorMVC - flexormin);
     extens = (extens_power - extensmin) / (extensMVC - extensmin);
+    //make sure values stay between 0-1 over time
+    keep_in_range(&biceps,0,1);
+    keep_in_range(&triceps,0,1);
+    keep_in_range(&flexor,0,1);
+    keep_in_range(&extens,0,1);
+    
     
     scope.set(2,biceps);
     scope.set(3,triceps);
@@ -795,32 +822,32 @@
     
     
     //threshold detection! buffer?
-    //TODO
+       //TODO
     
-  /*  //analyze emg (= velocity)
-    if (biceps>triceps && biceps > 0.1)
+    //analyze emg (= velocity)
+    if (biceps>triceps && biceps > 0.2){
         xdir = 0;
-        xpower = biceps;
-    else if (triceps>biceps && triceps>0.1)
+        xpower = biceps;}
+    else if (triceps>biceps && triceps>0.2){
         xdir = 1;
-        xpower = triceps;
+        xpower = triceps;}
     else 
         xpower=0;
         
-    if (flexor>extensor && flexor > 0.1){
+    if (flexor>extens && flexor > 0.2){
         ydir = 0;
         ypower = flexor;
         }
-    else if (extensor>flexor && extensor > 0.1){
+    else if (extens>flexor && extens > 0.2){
         ydir = 1;
-        ypower = extensor;
+        ypower = extens;
         }
     else
         ypower = 0;
             
     //power: the longer a signal is active, the further the reference goes. So integrate to determine reference position
-    dx = xpower * CONTROL_RATE;
-    dy = ypower * CONTROL_RATE; 
+    dx = xpower * CONTROL_RATE * 0.1;           //factor to slow or speed up
+    dy = ypower * CONTROL_RATE * 0.1; 
     
     //But: direction! Sum dx and dy but make sure xdir and ydir are considered.
     if (xdir>0)
@@ -833,29 +860,8 @@
     else
         y += -dy;
         
-   //now we have x and y -> reference position.     
-   
-   //Set limits to the error! 
-   //lower limit:   Negative error not allowed to go further.
-   if (theta1 < limitangle)
-        if (error1 > 0)
-            error1 = error1;
-        else 
-            error1 = 0; 
-   if (theta2 < limitangle)
-    same as above
-   
-   //upper limit: Positive error not allowed to go further
-   if (theta1 > limitangle)
-        if (error1 < 0 )
-            error1 = error1;
-        else
-            error1 = 0;
-   if (theta2 > limitangle)
-    same as above
-    
-             
-    */
+    //now we have x and y -> reference position.     
+    }//end emg_control if
         
     //Current position - Encoder counts -> current angle -> Forward Kinematics 
     rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
@@ -894,7 +900,7 @@
     //Angle error
     m1_error = dtheta1-theta1;
     m2_error = dtheta2-theta2;
-    }
+    }// end control method 1
     
     if(control_method==2){
     //inverse kinematics (error in position to error in angles)
@@ -909,7 +915,30 @@
     //Angle error
     m1_error = q1_error;
     m2_error = q2_error;
-    }
+    }//end control method 2
+    
+     /*  
+   //Set limits to the error! 
+   //lower limit:   Negative error not allowed to go further.
+   if (theta1 < limitangle)
+        if (error1 > 0)
+            error1 = error1;
+        else 
+            error1 = 0; 
+   if (theta2 < limitangle)
+    same as above
+   
+   //upper limit: Positive error not allowed to go further
+   if (theta1 > limitangle)
+        if (error1 < 0 )
+            error1 = error1;
+        else
+            error1 = 0;
+   if (theta2 > limitangle)
+    same as above
+    
+             
+    */
     
     //PID controller