Diff: main.cpp

Revision:

62:e400138d625e

Parent:

61:8226830f3272

Child:

63:08357f5c497b

--- a/main.cpp	Wed Oct 28 10:37:29 2015 +0000
+++ b/main.cpp	Thu Oct 29 22:35:13 2015 +0000
@@ -39,8 +39,8 @@
 Ticker      sample_timer;       //Ticker for EMG sampling
 Ticker      control_timer;      //Ticker for control loop
 Ticker      servo_timer;        //Ticker for servo control
-Ticker      debug_timer;
-//HIDScope    scope(2);           //Scope 4 channels
+Ticker      debug_timer;        //Ticker for debug printf
+//HIDScope    scope(2);         //Scope 4 channels
 
 // AnalogIn potmeter(A4);       //potmeters
 // AnalogIn potmeter2(A5);      //
@@ -87,13 +87,13 @@
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
-volatile bool debug = false;
+volatile bool debug = true;
 
 //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation. 
-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;
+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;
 
 //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=50;                           //100 samples
+const int window=30;                            //100 samples
 int i=0;                                        //movavg array index 
 double movavg1[window];                         //moving average arrays with size of window
 double movavg2[window];
@@ -117,10 +117,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 motor 1
-const double m1_kp=5; const double m1_ki=0.1; const double m1_kd=0.3;       //Proportional, integral and derivative gains.
+const double m1_kp=1; const double m1_ki=0.01; const double m1_kd=0.05;       //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;                     //Error, integrated error, previous error motor 2
-const double m2_kp=5; const double m2_ki=0.1; const double m2_kd=0.3;       //Proportional, integral and derivative gains.
+const double m2_kp=1; const double m2_ki=0.01; const double m2_kd=0.05;       //Proportional, integral and derivative gains.
 
 //Calibration bools, checks if elbow/shoulder limits are hit
 volatile bool done1 = false;
@@ -298,23 +298,13 @@
     pc.baud(115200);            //serial baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
-    servoPwm.Enable(600,20000); //Start position servo, PWM period in usecs
-    
-    //theta1_cal = floor(theta1_lower * (4200/(2*PI)));
-    //Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
-    
-    //Mechanical limit 43 degrees -> 43*(4200/360) = 350
-    //theta2_cal = floor(theta2_lower * (4200/(2*PI)));
-    //Encoder2.setPulses(theta2_cal);
+    servoPwm.Enable(602,20000); //Start position servo, PWM period in usecs
     
-    //x = 0.2220;
-    //y = 0.4088;
+    //Set PwmOut frequency to x Hz
+    pwm_motor1.period(0.02);    
+    pwm_motor2.period(0.02);
     
-    //Set PwmOut frequency to 10k Hz
-    pwm_motor1.period(0.001);    
-    pwm_motor2.period(0.001);
-    
-    debugbtn.fall(&debug_trigger);
+    debugbtn.fall(&debug_trigger);  //turn debug printf's on or off
     
     clearTerminal();            //Clear the putty window
     
@@ -373,7 +363,6 @@
             case 't':
             case 'T':
                 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);
@@ -386,7 +375,6 @@
             case 'd':
             case 'D':
                 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);
@@ -457,12 +445,12 @@
     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("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("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);
@@ -540,24 +528,7 @@
                                         
                       break;
     }//end switch
-    /* if(c!='q' && c!='Q'){
-    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("Pos Error: %f and %f \r\n",x_error,y_error);
-    pc.printf("Current 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 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");
-    pc.printf("Buffer 1: %f \r\n",biceps_avg);                
-    pc.printf("Buffer 2: %f \r\n",triceps_avg);
-    pc.printf("Buffer 3: %f \r\n",flexor_avg);
-    pc.printf("Buffer 4: %f \r\n",extens_avg);
-    pc.printf("----------------------------------------\r\n");
-    pc.printf("Theta 3: %f \r\n",theta3);
-    pc.printf("Servoposition (us): %f \r\n",servopos);
-    pc.printf("----------------------------------------\r\n");
-    }*/
+ 
     }
     //end if pc readable
     
@@ -597,8 +568,6 @@
     //scope.send();
        
 }
-
-
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
 void calibrate_arm(void)
@@ -668,12 +637,12 @@
 //Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker
 void calibrate(void){
     if(done1==false){                   //if motor 1 limit has not been hit yet
-            pwm_motor1=0.4;             //move upper arm slowly cw
+            pwm_motor1=0.1;             //move upper arm slowly cw
             pc.printf("Motor 1 running %f \r\n");
         }
     if(done1==true && done2==false){    //if limit motor 1 has been hit
             pwm_motor1=0;               //stop motor1
-            pwm_motor2=0.4;             //start moving forearm slowly cw
+            pwm_motor2=0.1;             //start moving forearm slowly cw
             pc.printf("Motor 2 running %f \r\n");    
         }   
 }
@@ -683,13 +652,13 @@
 {
    Ticker timer;
    
-   pc.printf("Starting sampling, to allow hidscope to normalize\r\n");
+   pc.printf("Starting sampling, to allow EMG to normalize\r\n");
    start_sampling();
-   wait(1);
+   wait(0.5);
    
    /******************* All muscles: minimum measurement *************************/
    pc.printf("Start of minimum measurement, relax all muscles.\r\n");
-   wait(1);
+   wait(0.5);
    pc.printf(" Press any key to begin... "); wait(1);
    char input;
    input=pc.getc();
@@ -701,22 +670,22 @@
    wait(3);
    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);
+   pc.printf("\r\n Right Flexor min = %f \r\n",bicepsmin); 
+   pc.printf("\r\n Right Extensor min = %f \r\n",tricepsmin); 
+   pc.printf("\r\n Left Flexor min = %f \r\n",flexormin); 
+   pc.printf("\r\n Left Extensor min = %f \r\n",extensmin); wait(1);
    /******************************** Done ****************************************/
    
-   pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(1); 
+   pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(0.5); 
    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(1);
    calibrate_time=0;
    
    /********************* 1st channel: MVC measurement ***************************/
-   pc.printf("\r\n----------------\r\n "); 
-   pc.printf(" Biceps is first.\r\n "); 
-   pc.printf("----------------\r\n "); 
+   pc.printf("\r\n---------------------\r\n "); 
+   pc.printf("Right Flexor is first.\r\n "); 
+   pc.printf("--------------------\r\n "); 
    wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    input=pc.getc();
@@ -731,17 +700,16 @@
    timer.detach();
   
    pc.printf("\r\n Measurement complete."); wait(1);
-   pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1);
-      pc.printf("Calibrate_emg() exited \r\n"); wait(1);
+   pc.printf("\r\n Right Flexor MVC = %f \r\n",bicepsMVC); wait(1);
    pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
    calibrate_time=0;
    /******************************** Done ****************************************/
    
    /********************* 2nd channel: MVC measurement ***************************/
    muscle=2;
-   pc.printf("\r\n----------------\r\n "); 
-   pc.printf(" Triceps is next.\r\n "); 
-   pc.printf("----------------\r\n "); 
+   pc.printf("\r\n-------------------\r\n "); 
+   pc.printf("Right Extensor is next.\r\n "); 
+   pc.printf("---------------------\r\n "); 
    wait(1);
 
    pc.printf(" Press any key to begin... "); wait(1);
@@ -754,18 +722,17 @@
    timer.attach(&emg_mvc,0.002);
    wait(3);
    timer.detach();
-   pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
+   pc.printf("\r\n Right Extensor MVC = %f \r\n",tricepsMVC);
    
-   pc.printf("Calibrate_emg() exited \r\n");
    pc.printf("Measured time: %f seconds \r\n",calibrate_time);
    calibrate_time=0;
    /******************************** Done ****************************************/
       
    /********************* 3rd channel: MVC measurement ***************************/
    muscle=3;
-   pc.printf("\r\n----------------\r\n "); 
-   pc.printf(" Flexor is next.\r\n "); 
-   pc.printf("----------------\r\n "); 
+   pc.printf("\r\n--------------------\r\n "); 
+   pc.printf("Left Flexor is next.\r\n "); 
+   pc.printf("--------------------\r\n "); 
    wait(1);
 
    pc.printf(" Press any key to begin... "); wait(1);
@@ -778,18 +745,17 @@
    timer.attach(&emg_mvc,0.002);
    wait(3);
    timer.detach();
-   pc.printf("\r\n Flexor MVC = %f \r\n",flexorMVC);
+   pc.printf("\r\n Left 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;
    /******************************** Done ****************************************/
    
    /********************* 4th channel: MVC measurement ***************************/
    muscle=4;
-   pc.printf("\r\n----------------\r\n "); 
-   pc.printf(" Extensor is next.\r\n "); 
-   pc.printf("----------------\r\n "); 
+   pc.printf("\r\n--------------------\r\n "); 
+   pc.printf("Left Extensor is next.\r\n "); 
+   pc.printf("--------------------\r\n "); 
    wait(1);
 
    pc.printf(" Press any key to begin... "); wait(1);
@@ -802,9 +768,8 @@
    timer.attach(&emg_mvc,0.002);
    wait(3);
    timer.detach();
-   pc.printf("\r\n Extensor MVC = %f \r\n",extensMVC);
+   pc.printf("\r\n Left 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;
    /******************************** Done ****************************************/
@@ -821,42 +786,42 @@
         if(muscle==1){
             
             if(biceps_power>bicepsMVC){
-            //printf("+ ");
-            printf("%s+ %s",GREEN_,_GREEN);
+            //pc.printf("+ ");
+            pc.printf("%s+ %s",GREEN_,_GREEN);
             bicepsMVC=biceps_power;
             }    
             else
-            printf("- ");
+            pc.printf("- ");
         }  
         
         if(muscle==2){
             
             if(triceps_power>tricepsMVC){
-            printf("%s+ %s",GREEN_,_GREEN);
+            pc.printf("%s+ %s",GREEN_,_GREEN);
             tricepsMVC=triceps_power;
             }    
             else
-            printf("- ");
+            pc.printf("- ");
         }
         
         if(muscle==3){
             
             if(flexor_power>flexorMVC){
-            printf("%s+ %s",GREEN_,_GREEN);    
+            pc.printf("%s+ %s",GREEN_,_GREEN);    
             flexorMVC=flexor_power;
             }    
-            else
-            printf("- ");
+            //else
+            pc.printf("- ");
         }
         
         if(muscle==4){
             
             if(extens_power>extensMVC){
-            printf("%s+ %s",GREEN_,_GREEN);    
+            pc.printf("%s+ %s",GREEN_,_GREEN);    
             extensMVC=extens_power;
             }    
-            else
-            printf("- ");
+            //else
+            pc.printf("- ");
         }
         
     //}
@@ -962,11 +927,15 @@
     extens_avg = extens_avg/window;
     
     emg_control_time += CONTROL_RATE; 
+    
+    //Move mouse to starting position - bottom right corner - when EMG control starts. After 5 seconds reference can be changed with EMG.
     if(emg_control_time < 5){
-        x=0; y=0.4; 
+        
+        x=0; y=0.3;             
     }
     else{
-    
+        
+        
     //Compare muscle amplitudes and determine their influence on x and y reference position.
     if (biceps_avg>triceps_avg && biceps_avg > 0.2){
         xdir = 0;                            
@@ -989,8 +958,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.1;           //last value is a factor to control how fast the reference (so also end effector) changes 
-    dy = ypower * CONTROL_RATE * 0.1; 
+    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; 
     
     //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
@@ -1004,9 +973,9 @@
         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);
+    //now we have x and y -> reference position. Keep in desired range.
+    keep_in_range(&x,-0.5,0);   
+    keep_in_range(&y,0.2,0.55);
     
     }//end emg_control if
     /******************************** END OF EMG REFERENCE CALCULATION ****************************************/
@@ -1114,8 +1083,8 @@
     u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);    //motor 2
     
     //keep u between limits, sign = direction, PWM = 0-1
-    keep_in_range(&u1,-0.7,0.7);    
-    keep_in_range(&u2,-0.7,0.7);
+    keep_in_range(&u1,-0.3,0.3);    
+    keep_in_range(&u2,-0.3,0.3);
     
     //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.