Diff: main.cpp

Revision:

50:54f71544964c

Parent:

49:6515c045cfd6

Child:

51:e4a0ce7ff4b8

--- a/main.cpp	Fri Oct 23 21:09:32 2015 +0000
+++ b/main.cpp	Tue Oct 27 08:44:29 2015 +0000
@@ -112,10 +112,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=8; const double m1_ki=0.125; const double m1_kd=0.5;       //Proportional, integral and derivative gains.
+const double m1_kp=10; const double m1_ki=0.25; const double m1_kd=0.5;       //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=8; const double m2_ki=0.125; const double m2_kd=0.5;       //Proportional, integral and derivative gains.
+const double m2_kp=5; const double m2_ki=0.125; const double m2_kd=0.25;       //Proportional, integral and derivative gains.
 
 //Calibration bools, checks if elbow/shoulder limits are hit
 volatile bool done1 = false;
@@ -279,15 +279,15 @@
     pc.baud(115200);            //serial baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
-    theta1_cal = floor(theta1_lower * (4200/(2*PI)));
-    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
+    //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);
+    //theta2_cal = floor(theta2_lower * (4200/(2*PI)));
+    //Encoder2.setPulses(theta2_cal);
     
-    x = 0.2220;
-    y = 0.4088;
+    //x = 0.2220;
+    //y = 0.4088;
     
     //Set PwmOut frequency to 10k Hz
     pwm_motor1.period(0.001);    
@@ -431,8 +431,24 @@
     controlMenu();
     char c=0;
     while(c != 'Q' && c != 'q') {
+    //Debugging values:
+    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\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);
+    wait(1);
+    }//end if
     
-    if( pc.readable() ){
+    /*if( pc.readable() ){
         c = pc.getc();
         switch (c) 
             {
@@ -470,18 +486,10 @@
                       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\n");
-    }//end if
     }
     //end if pc readable
+    */
+    
     }
     //end of while loop
 }
@@ -503,11 +511,11 @@
     biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
    
     
-    scope.set(0,biceps_power);
-    scope.set(1,triceps_power);
+    //scope.set(0,biceps_power);
+    //scope.set(1,triceps_power);
     //scope.set(2,flexor_power);
     //scope.set(3,extens_power);
-    scope.send();
+    //scope.send();
        
 }
 
@@ -536,12 +544,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.1;             //move upper arm slowly cw
+            pwm_motor1=0.3;             //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.1;             //start moving forearm slowly cw
+            pwm_motor2=0.3;             //start moving forearm slowly cw
             pc.printf("Motor 2 running %f \r\n");    
         }   
   //  if(done1==true && done2==true)      //if both limits are hit
@@ -832,13 +840,11 @@
 
 //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
+    //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    
@@ -856,13 +862,15 @@
     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.set(0,biceps);
+    scope.set(1,triceps);
+    scope.set(2,flexor);
+    scope.set(3,extens);
     scope.send();
     
     //threshold detection! buffer or two thresholds? If average of 100 samples > threshold, then muscle considered on.
      
-    /*
+    
     movavg1[i]=biceps;   //fill array with 100 normalized samples
     movavg2[i]=triceps;
     movavg3[i]=flexor;
@@ -872,18 +880,22 @@
         i=0;
     }
     
-    for(int j = 0; j < window; j++){        // sum all values in the array
-        sum1 += movavg1[j];
-        sum2 += movavg2[j];
-        sum3 += movavg3[j];
-        sum4 += movavg4[j];
+    
+    //Sum all values in the array. The sum needs to be overwritten or it will continue to sum the next 100 samples on top it 
+    //and will grow out of control.
+    //So the variable name for the sum in the for loop is not really correct since the average is calculated after the loop.
+    for(int j = 0; j < window; j++){        
+        biceps_avg += movavg1[j];
+        triceps_avg += movavg2[j];
+        flexor_avg += movavg3[j];
+        extens_avg += movavg4[j];
         }
-    biceps_avg = sum1/window;                       //divide sum by number of samples -> average
-    triceps_avg = sum2/window;
-    flexor_avg = sum3/window;
-    extens_avg = sum4/window;
+    biceps_avg = biceps_avg/window;                       //divide sum by number of samples -> average
+    triceps_avg = triceps_avg/window;
+    flexor_avg = flexor_avg/window;
+    extens_avg = extens_avg/window;
     
-    */
+    
     
     //Compare muscle amplitudes and determine their influence on x and y reference position.
     if (biceps>triceps && biceps > 0.2){
@@ -939,13 +951,13 @@
     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) ) );                //absolute to avoid imaginary numbers -> bigger steady state error
+    //absolute in sqrt to avoid imaginary numbers -> bigger steady state error when reference out of workspace
+    sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );                
     
     //Reference angle 2 
     dtheta2 = atan2(sintheta2,costheta2);