Revision 38:c8ac615d0c8f, committed 2015-10-23

Comitter:

Vigilance88

Date:

Fri Oct 23 00:02:19 2015 +0000

Parent:

37:4d7b7ced20ef

Child:

39:e77f844d10d9

Commit message:

normalizing emg works

Changed in this revision

--- a/main.cpp	Thu Oct 22 23:26:37 2015 +0000
+++ b/main.cpp	Fri Oct 23 00:02:19 2015 +0000
@@ -85,10 +85,12 @@
 
 //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - minimum amplitude during relaxation. 
 //minimum declared as 1 so the comparison with EMG during calibration works correctly - function emg_min()
-double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=1;
-double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=1;
-double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=1;
-double emg_extens; double extens_power; double extensMVC = 0; double extensmin=1;
+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;
+//Normalized emg values
+double biceps, triceps, flexor, extens;
 
 int muscle;             //Muscle selector for MVC measurement, 1 = first emg etc.
 double calibrate_time;  //Elapsed time for each MVC measurement
@@ -159,6 +161,9 @@
 //Reference position
 double x; double y;
 
+//Select whether to use Trig or DLS method
+int control_method;
+
 //Inverse Kinematics - Trig / Gonio method. 
 //First convert reference position to angle needed, then compare that angle to current angle.
 double dtheta1; double dtheta2;                     //reference angles
@@ -324,6 +329,7 @@
                 wait(1);
                 start_sampling();
                 wait(1);
+                control_method=1;
                 start_control();
                 wait(1);   
                 controlButtons();
@@ -334,7 +340,8 @@
                 wait(1);
                 start_sampling();
                 wait(1);
-                start_dlscontrol();
+                control_method=2;
+                start_control();
                 wait(1);   
                 controlButtons();
                 break;    
@@ -387,23 +394,23 @@
         c = pc.getc();
         switch (c) 
             {
-            case '1' :  
+            case 'd' :  
                      x = x + 0.01; 
                     
                      break;
           
-            case '2' :
+            case 'a' :
                      x-=0.01;
 
                      break;
                     
-            case '3' :
+            case 'w' :
                      y+=0.01;
 
                      break;
                      
           
-            case '4' :
+            case 's' :
                      y-=0.01;
 
                      break;
@@ -456,8 +463,8 @@
     
     scope.set(0,biceps_power);
     scope.set(1,triceps_power);
-    scope.set(2,flexor_power);
-    scope.set(3,extens_power);
+    //scope.set(2,flexor_power);
+    //scope.set(3,extens_power);
     scope.send();
     
     // on - offset. If above a value it is on. 
@@ -607,19 +614,19 @@
 void emg_min()
 {  
             
-            if(biceps_power<bicepsmin){
+            if(biceps_power>bicepsmin){
             bicepsmin=biceps_power;
             }    
             
-            if(triceps_power<tricepsmin){
+            if(triceps_power>tricepsmin){
             tricepsmin=triceps_power;
             }            
 
-            if(flexor_power<flexormin){
+            if(flexor_power>flexormin){
             flexormin=flexor_power;
             }            
  
-            if(extens_power < extensmin){   
+            if(extens_power > extensmin){   
             extensmin = extens_power;
             }    
         
@@ -632,9 +639,10 @@
 {
    Ticker timer;
    
-   pc.printf("Testcode calibration \r\n");
+   pc.printf("Starting sampling, to allow hidscope to normalize\r\n");
+   start_sampling();
    wait(1);
-   pc.printf("Starting minimum measurement, relax all muscles.\r\n");
+   pc.printf("Start minimum measurement, relax all muscles.\r\n");
    wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    char input;
@@ -643,7 +651,7 @@
    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);
@@ -774,18 +782,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()
 {
-   /* 
-   
+     
     //normalize emg to value between 0-1
-    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)
+    biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
+    triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
+    flexor = (flexor_power - flexormin) / (flexorMVC - flexormin);
+    extens = (extens_power - extensmin) / (extensMVC - extensmin);
+    
+    scope.set(2,biceps);
+    scope.set(3,triceps);
+    scope.send();
+    
     
     //threshold detection! buffer?
-    TODO
+    //TODO
     
-    //analyze emg (= velocity)
+  /*  //analyze emg (= velocity)
     if (biceps>triceps && biceps > 0.1)
         xdir = 0;
         xpower = biceps;
@@ -806,7 +818,7 @@
     else
         ypower = 0;
             
-    //We have power: the longer a signal is active, the further you go. So integrate to determine reference position
+    //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; 
     
@@ -858,7 +870,7 @@
     y_error = y - current_y;
     
     
-    if (control_method=1){
+    if (control_method==1){
     //inverse kinematics (refpos to refangle)
     
     costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
@@ -884,7 +896,7 @@
     m2_error = dtheta2-theta2;
     }
     
-    if(control_method=2){
+    if(control_method==2){
     //inverse kinematics (error in position to error in angles)
     dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
     dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
@@ -936,143 +948,11 @@
     
 }
 
-void dlscontrol()
-{
-   /* 
-   
-    //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)
-    
-    //analyze emg (= velocity)
-    if (biceps>triceps && biceps > 0.1)
-        xdir = 0;
-        xpower = biceps;
-    else if (triceps>biceps && triceps>0.1)
-        xdir = 1;
-        xpower = triceps;
-    else 
-        xpower=0;
-        
-    if (flexor>extensor && flexor > 0.1){
-        ydir = 0;
-        ypower = flexor;
-        }
-    else if (extensor>flexor && extensor > 0.1){
-        ydir = 1;
-        ypower = extensor;
-        }
-    else
-        ypower = 0;
-            
-    //We have power: the longer a signal is active, the further you go. So integrate to determine reference position
-    dx = xpower * CONTROL_RATE;
-    dy = ypower * CONTROL_RATE; 
-    
-    //But: direction! Sum dx and dy but make sure xdir and ydir are considered.
-    if (xdir>0)
-        x += dx;
-    else 
-        x += -dx;
-        
-    if (ydir>0)
-        y += dy;
-    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
-    
-             
-    */
-        
-    //Current position - Encoder counts -> current angle -> Forward Kinematics 
-    rpc=(2*PI)/ENCODER1_CPR;               //radians per count (resolution) - 2pi divided by 4200
-    theta1 = Encoder1.getPulses() * rpc;   //multiply resolution with number of counts
-    theta2 = Encoder2.getPulses() * rpc;
-    current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
-    current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
-    
-        
-    //calculate error (refpos-currentpos) currentpos = forward kinematics
-    x_error = x - current_x;
-    y_error = y - current_y;
-    
-    
-    //inverse kinematics (error in position to error in angles)
-    dls1= -(l2*pow(lambda,2)*sin(theta1 + theta2) + l1*pow(lambda,2)*sin(theta1) + l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) - l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1) + pow(l1,2)*l2*sin(theta1 + theta2)*pow(cos(theta1),2) - pow(l1,2)*l2*cos(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*pow(sin(theta1 + theta2),2)*cos(theta1) + pow(l1,2)*l2*cos(theta1 + theta2)*pow(sin(theta1),2) - pow(l1,2)*l2*sin(theta1 + theta2)*cos(theta1)*sin(theta1) + l1*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*sin(theta1))/(pow(lambda,4) + 2*pow(l2,2)*pow(lambda,2)*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*pow(cos(theta1),2) + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(sin(theta1 + theta2),2)*pow(cos(theta1),2) + 2*l1*l2*pow(lambda,2)*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*pow(lambda,2)*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    
-    q1_error = dls1 * x_error + dls2 * y_error;
-    q2_error = dls3 * x_error + dls4 * y_error;
-    
-    //Angle error
-    m1_error = q1_error;
-    m2_error = q2_error;
-    
-    
-    //PID controller
-    
-    u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);    //motor 1
-    u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
-    
-    keep_in_range(&u1,-0.6,0.6);    //keep u between -1 and 1, sign = direction, PWM = 0-1
-    keep_in_range(&u2,-0.6,0.6);
-    
-    //send PWM and DIR to motor 1
-    dir_motor1 = u1>0 ? 1 : 0;          //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. 
-    pwm_motor1.write(abs(u1));
-    
-    //send PWM and DIR to motor 2
-    dir_motor2 = u2>0 ? 0 : 1;          //conditional statement, same as if else below
-    pwm_motor2.write(abs(u2));
-    
-    /*if(u1 > 0)
-    {
-        dir_motor1 = 0;
-    else{
-        dir_motor1 = 1;
-        }
-    }    
-    pwm_motor1.write(abs(u1));
-    
-  
-    if(u2 > 0)
-    {
-        dir_motor1 = 1;
-    else{
-        dir_motor1 = 0;
-        }
-    }    
-    pwm_motor1.write(abs(u2));*/
-    
-}
+
 
 void mainMenu()
 {
-       //Title Box
+   //Title Box
    pc.putc(201); 
    for(int j=0;j<33;j++){
    pc.putc(205);
@@ -1130,16 +1010,6 @@
     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.
-    blue=0;
-    pc.printf("||- started control -|| \r\n");
-}
-
-
 //stop control
 void stop_control(void)
 {
@@ -1151,11 +1021,6 @@
 }
 
 
-void calibrate()
-{
-
-}
-
 //Clears the putty (or other terminal) window
 void clearTerminal()
 {
@@ -1185,10 +1050,10 @@
    
    pc.printf("\n\r");
    //endbox
-     pc.printf("1) Move Arm Left\r\n");
-     pc.printf("2) Move Arm Right\r\n");
-     pc.printf("3) Move Arm Up\r\n");
-     pc.printf("4) Move Arm Down\r\n");
+     pc.printf("A) Move Arm Left\r\n");
+     pc.printf("D) Move Arm Right\r\n");
+     pc.printf("W) Move Arm Up\r\n");
+     pc.printf("S) Move Arm Down\r\n");
      pc.printf("q) Exit \r\n");
      pc.printf("Please make a choice => \r\n");
 }
@@ -1246,11 +1111,11 @@
 void emgInstructions(){
     pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); 
     pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);
-    pc.printf("\n\r Check if EMG signal looks normal \n\r "); wait(1);
-    pc.printf("\n\r To calibrate the EMG signals we will measure: \n\r ");
-    pc.printf("\n\r - Minimum amplitude, while relaxing all muscles. \n\r ");
-    pc.printf("\n\r - Maximum Voluntary Contraction of each muscle. \n\r"); wait(2);
-    pc.printf("For the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1);
+    pc.printf("\r\n Check whether EMG signal looks normal. \n\r "); wait(1);
+    pc.printf("\r\n To calibrate the EMG signals we will measure: \n\r ");
+    pc.printf("- Minimum amplitude, while relaxing all muscles. \n\r ");
+    pc.printf("- Maximum Voluntary Contraction of each muscle. \n\r"); wait(2);
+    pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1);
     pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1);
 }