Revision 61:8226830f3272, committed 2015-10-28

Comitter:

Vigilance88

Date:

Wed Oct 28 10:37:29 2015 +0000

Parent:

60:6541eec8d6ad

Child:

62:e400138d625e

Commit message:

added emg control time if then

Changed in this revision

--- a/main.cpp	Wed Oct 28 09:09:55 2015 +0000
+++ b/main.cpp	Wed Oct 28 10:37:29 2015 +0000
@@ -90,10 +90,10 @@
 volatile bool debug = false;
 
 //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation. 
-double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0;  
-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;
+double emg_biceps; double biceps_power; double bicepsMVC = 0.1227; double bicepsmin=0.024;  
+double emg_triceps; double triceps_power; double tricepsMVC = 0.094; double tricepsmin=0.023;
+double emg_flexor; double flexor_power; double flexorMVC = 0.096; double flexormin=0.041;
+double emg_extens; double extens_power; double extensMVC = 0.124; double extensmin=0.037;
 
 //Normalize and compare variables
 double biceps, triceps, flexor, extens;         //Storage for normalized emg
@@ -103,7 +103,7 @@
 double emg_control_time;                        //Elapsed time in EMG control
 
 //Threshold moving average
-const int window=100;                           //100 samples
+const int window=50;                           //100 samples
 int i=0;                                        //movavg array index 
 double movavg1[window];                         //moving average arrays with size of window
 double movavg2[window];
@@ -457,20 +457,20 @@
     if(debug==true){
     //Debugging values:
     pc.printf("\r\nRef pos: %f and %f \r\n",x,y);
-    pc.printf("Cur pos: %f and %f \r\n",current_x,current_y);
-    pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
+    //pc.printf("Cur pos: %f and %f \r\n",current_x,current_y);
+    //pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
     //pc.printf("Cur angles: %f and %f \r\n",theta1,theta2);
     //pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
-    pc.printf("Error angles: %f and %f \r\n",m1_error,m2_error);
-    pc.printf("PID output: %f and %f \r\n",u1,u2);
+    //pc.printf("Error angles: %f and %f \r\n",m1_error,m2_error);
+    //pc.printf("PID output: %f and %f \r\n",u1,u2);
     pc.printf("----------------------------------------\r\n");
     pc.printf("Buffer1: %f \r\n",biceps_avg);                
     pc.printf("Buffer2: %f \r\n",triceps_avg);
     pc.printf("Buffer3: %f \r\n",flexor_avg);
     pc.printf("Buffer4: %f \r\n",extens_avg);
-    pc.printf("----------------------------------------\r\n");
-    pc.printf("Theta3: %f \r\n",theta3);
-    pc.printf("Servopos us: %f \r\n",servopos);
+    //pc.printf("----------------------------------------\r\n");
+    //pc.printf("Theta3: %f \r\n",theta3);
+    //pc.printf("Servopos us: %f \r\n",servopos);
     pc.printf("----------------------------------------\r\n");
     
     }
@@ -923,11 +923,7 @@
    /********************* START OF EMG REFERENCE CALCULATION ***************************/
     if(emg_control==true){
     //TODO some idle time with static reference before EMG kicks in. or go to reference in the first 5 seconds.
-    emg_control_time += CONTROL_RATE; 
-    //if(emg_control_time < 5){
-    //    x=BLA; y=BLA; starting position maybe    
-    //}
-    
+
     //normalize emg to value between 0-1
     biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
     triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
@@ -965,7 +961,11 @@
     flexor_avg = flexor_avg/window;
     extens_avg = extens_avg/window;
     
-    
+    emg_control_time += CONTROL_RATE; 
+    if(emg_control_time < 5){
+        x=0; y=0.4; 
+    }
+    else{
     
     //Compare muscle amplitudes and determine their influence on x and y reference position.
     if (biceps_avg>triceps_avg && biceps_avg > 0.2){
@@ -989,8 +989,8 @@
         ypower = 0;
             
     //power: the longer a signal is active, the further the reference goes. So integrate to determine reference position
-    dx = xpower * CONTROL_RATE * 0.15;           //last value is a factor to control how fast the reference (so also end effector) changes 
-    dy = ypower * CONTROL_RATE * 0.15; 
+    dx = xpower * CONTROL_RATE * 0.1;           //last value is a factor to control how fast the reference (so also end effector) changes 
+    dy = ypower * CONTROL_RATE * 0.1; 
     
     //Direction! Sum dx and dy but make sure xdir and ydir are considered.
     if (xdir>0)     //if x direction of sample is positive, add it to reference position
@@ -1002,9 +1002,12 @@
         y += dy;
     else
         y += -dy;   //if y direction of sample is negative, substract it from reference position
+   }//end else control time>5
         
     //now we have x and y -> reference position. 
-        
+    //keep_in_range(&x,-0.4,0.22);   
+    //keep_in_range(&y,0,0.5);
+    
     }//end emg_control if
     /******************************** END OF EMG REFERENCE CALCULATION ****************************************/