Diff: main.cpp

Revision:

48:a1f97eb8c020

Parent:

47:c52f9b4c90c4

Child:

49:6515c045cfd6

--- a/main.cpp	Fri Oct 23 20:13:06 2015 +0000
+++ b/main.cpp	Fri Oct 23 20:40:31 2015 +0000
@@ -643,8 +643,7 @@
 }
 
 void emg_min()
-{  
-            
+{          
             if(biceps_power>bicepsmin){
             bicepsmin=biceps_power;
             }    
@@ -673,7 +672,9 @@
    pc.printf("Starting sampling, to allow hidscope to normalize\r\n");
    start_sampling();
    wait(1);
-   pc.printf("Start minimum measurement, relax all muscles.\r\n");
+   
+   /******************* All muscles: minimum measurement *************************/
+   pc.printf("Start of minimum measurement, relax all muscles.\r\n");
    wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    char input;
@@ -682,8 +683,6 @@
    pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
    pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
    
-   
-   
    timer.attach(&emg_min,SAMPLE_RATE);
    wait(5);
    timer.detach();
@@ -692,12 +691,15 @@
    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);
+   /******************************** Done ****************************************/
    
-   calibrate_time=0;
-      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(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);
+   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 "); 
@@ -709,7 +711,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,SAMPLE_RATE);
    wait(5);
@@ -720,8 +721,9 @@
       pc.printf("Calibrate_emg() exited \r\n"); wait(1);
    pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
    calibrate_time=0;
+   /******************************** Done ****************************************/
    
-   // Triceps:
+   /********************* 2nd channel: MVC measurement ***************************/
    muscle=2;
    pc.printf("\r\n----------------\r\n "); 
    pc.printf(" Triceps is next.\r\n "); 
@@ -734,7 +736,7 @@
    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();
@@ -743,8 +745,9 @@
    pc.printf("Calibrate_emg() exited \r\n");
    pc.printf("Measured time: %f seconds \r\n",calibrate_time);
    calibrate_time=0;
-   
-   //Flexor:
+   /******************************** Done ****************************************/
+      
+   /********************* 3rd channel: MVC measurement ***************************/
    muscle=3;
    pc.printf("\r\n----------------\r\n "); 
    pc.printf(" Flexor is next.\r\n "); 
@@ -757,7 +760,7 @@
    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();
@@ -766,9 +769,9 @@
    pc.printf("Calibrate_emg() exited \r\n");
    pc.printf("Measured time: %f seconds \r\n",calibrate_time);
    calibrate_time=0;
+   /******************************** Done ****************************************/
    
-   //Extensor:
-   
+   /********************* 4th channel: MVC measurement ***************************/
    muscle=4;
    pc.printf("\r\n----------------\r\n "); 
    pc.printf(" Extensor is next.\r\n "); 
@@ -781,7 +784,7 @@
    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();
@@ -790,18 +793,20 @@
    pc.printf("Calibrate_emg() exited \r\n");
    pc.printf("Measured time: %f seconds \r\n",calibrate_time);
    calibrate_time=0;
-   //Stop sampling, detach ticker
+   /******************************** Done ****************************************/
+   
+   //Stop sampling: detach ticker
    stop_sampling();
    
 }
 
 
-//Input error and Kp, Kd, Ki, output control signal
+//PID motor 1 - Input error and Kp, Kd, Ki, output control signal
 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
 {
     // Derivative
     double e_der = (error-e_prev)/ CONTROL_RATE;
-    e_der = derfilter1.step(e_der);
+    e_der = derfilter1.step(e_der);                 //derfilter1 - seperate 7hz low-pass biquad for this PID 
     e_prev = error;
     // Integral
     e_int = e_int + CONTROL_RATE * error;
@@ -810,12 +815,12 @@
  
 }
 
-//Input error and Kp, Kd, Ki, output control signal
+//PID for motor 2 - needed because of biquadfilter memory variables? 
 double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
 {
     // Derivative
     double e_der = (error-e_prev)/ CONTROL_RATE;
-    e_der = derfilter2.step(e_der);
+    e_der = derfilter2.step(e_der);                 //derfilter2 - seperate 7hz low-pass biquad for this PID 
     e_prev = error;
     // Integral
     e_int = e_int + CONTROL_RATE * error;
@@ -828,12 +833,17 @@
 //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()
 { 
+    
+      
+      
+   /********************* START OF EMG REFERENCE CALCULATION ***************************/
     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){
-    //    x=0; y=0;    
+    //    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);
@@ -845,7 +855,7 @@
     keep_in_range(&flexor,0,1);
     keep_in_range(&extens,0,1);
     
-    
+    //send normalized emg to scope to compare with just filtered emg.
     scope.set(2,biceps);
     scope.set(3,triceps);
     scope.send();
@@ -874,9 +884,10 @@
     extens_avg = sum4/window;
     
     */
-    //analyze emg (= velocity)
+    
+    //Compare muscle amplitudes and determine their influence on x and y reference position.
     if (biceps>triceps && biceps > 0.2){
-        xdir = 0;
+        xdir = 0;                            
         xpower = biceps;}
     else if (triceps>biceps && triceps>0.2){
         xdir = 1;
@@ -896,48 +907,53 @@
         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;           //factor to slow or speed up
+    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; 
     
-    //But: direction! Sum dx and dy but make sure xdir and ydir are considered.
-    if (xdir>0)
-        x += dx;
-    else 
+    //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
+        x += dx;    
+    else            //if x direction of sample is negative, substract it from reference position
         x += -dx;
         
-    if (ydir>0)
+    if (ydir>0)     //if y direction of sample is positive, add it to reference position
         y += dy;
     else
-        y += -dy;
+        y += -dy;   //if y direction of sample is negative, substract it from reference position
+        
+    //now we have x and y -> reference position. 
         
-    //now we have x and y -> reference position.     
     }//end emg_control if
+    /******************************** END OF EMG REFERENCE CALCULATION ****************************************/
+    
         
     //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
+    theta1 = Encoder1.getPulses() * rpc;   //multiply resolution with number of counts to get current angles
     theta2 = Encoder2.getPulses() * rpc;
-    current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
+    current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);   //Forward kinematics for current position
     current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
     
         
-    //calculate error (refpos-currentpos) currentpos = forward kinematics
+    //calculate error (refpos-currentpos) 
     x_error = x - current_x;
     y_error = y - current_y;
     
     
+    /******************************** START OF TRIG INVERSE KINEMATICS ****************************************/    
     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) ;
-    sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
-     
+    sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );                //absolute to avoid imaginary numbers -> bigger steady state error
     
+    //Reference angle 2 
     dtheta2 = atan2(sintheta2,costheta2);
     
     double k1 = l1 + l2*costheta2;
     double k2 = l2*sintheta2;
     
+    //Reference angle 1
     dtheta1 = atan2(y, x) - atan2(k2, k1);
     
     /* alternative:
@@ -951,7 +967,10 @@
     m1_error = dtheta1-theta1;
     m2_error = dtheta2-theta2;
     }// end control method 1
+    /******************************** END OF TRIG INVERSE KINEMATICS ****************************************/
     
+    
+    /******************************** START OF DLS INVERSE KINEMATICS ****************************************/
     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));
@@ -966,13 +985,14 @@
     m1_error = q1_error;
     m2_error = q2_error;
     }//end control method 2
+    /******************************** END OF DLS INVERSE KINEMATICS ****************************************/
     
-    /*  
-   //Set limits to the error! 
-   motor1: lower limit 40 degrees, upper limit 135
-   motor2: lower 43 degrees, upper limit 138 degrees. */
+    
+    /* Set limits to the error! 
+    motor1: lower limit 40 degrees, upper limit 135
+    motor2: lower 43 degrees, upper limit 138 degrees. */
    
-   //lower limits:   Negative error not allowed to go further.
+   //lower limits:   Negative error not allowed to go further. NEEDS MORE TESTING
    if (theta1 < theta1_lower){
         if (m1_error > 0)
             m1_error = m1_error;
@@ -999,11 +1019,11 @@
    }
              
     //PID controller
-    
     u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);    //motor 1
     u2=pid2(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 u between limits, sign = direction, PWM = 0-1
+    keep_in_range(&u1,-0.6,0.6);    
     keep_in_range(&u2,-0.6,0.6);
     
     //send PWM and DIR to motor 1
@@ -1036,7 +1056,6 @@
 }
 
 
-
 void mainMenu()
 {
    //Title Box
@@ -1056,6 +1075,7 @@
    
    pc.printf("\n\r");
    //endbox
+   
    wait(1);
    pc.printf("[C]alibration\r\n"); wait(0.2);
    pc.printf("[T]RIG Control with WASD\r\n"); wait(0.2);
@@ -1121,7 +1141,7 @@
 
 void controlMenu()
 {
-         //Title Box
+   //Title Box
    pc.putc(201); 
    for(int j=0;j<33;j++){
    pc.putc(205);
@@ -1138,6 +1158,7 @@
    
    pc.printf("\n\r");
    //endbox
+     
      pc.printf("A) Move Arm Left\r\n");
      pc.printf("D) Move Arm Right\r\n");
      pc.printf("W) Move Arm Up\r\n");
@@ -1148,7 +1169,7 @@
 
 void caliMenu(){
      
-        //Title Box
+   //Title Box
    pc.putc(201); 
    for(int j=0;j<33;j++){
    pc.putc(205);
@@ -1202,9 +1223,9 @@
     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);
+    pc.printf("- Maximum Voluntary Contraction of each muscle. \n\r"); wait(1);
+    pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(0.5);
+    pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(0.5);
 }
 
 //keeps input limited between min max