Diff: main.cpp

Revision:

58:db11481da856

Parent:

57:d6192801fd6d

Child:

59:ca85ce2b1ffc

diff -r d6192801fd6d -r db11481da856 main.cpp
--- a/main.cpp	Tue Oct 27 20:46:20 2015 +0000
+++ b/main.cpp	Wed Oct 28 07:45:42 2015 +0000
@@ -58,11 +58,11 @@
 DigitalOut dir_motor2(D4);      //Direction motor 2
 
 //Limit Switches
-InterruptIn shoulder_limit(D2);  //using FRDM buttons 
-InterruptIn elbow_limit(D3);     //using FRDM buttons
+InterruptIn shoulder_limit(D2);  //using BioShield buttons 
+InterruptIn elbow_limit(D3);     //using BioShield buttons 
 
 //Other buttons
-DigitalIn button1(PTA4);        //using FRDM buttons 
+InterruptIn debugbtn(PTA4);     //using FRDM buttons - debug on or off 
 DigitalIn button2(PTC6);        //using FRDM buttons
 
 /*Text colors ASCII code: Set Attribute Mode    <ESC>[{attr1};...;{attrn}m
@@ -199,7 +199,6 @@
 double reftheta1; double reftheta2;                     //reference angles
 double costheta1; double sintheta1;                 //helper variables
 double costheta2; double sintheta2;                 //    
-double powlambda2, powlambda4;
 
 //Inverse Kinematics - Damped least squares method. 
 //Angle error is directly calculated from position error: dq = [DLS matrix] * position_error
@@ -208,6 +207,9 @@
 double q1_error, q2_error;                          //Angle errors
 double x_error, y_error;                            //Position errors
 double lambda=0.1;                                  //Damping constant
+double powlambda2, powlambda4;
+double powl1, powl2;
+double sintheta1theta2, costheta1theta2;
 
 //Mechanical Limits
 int theta1_cal, theta2_cal;                         //Pulse counts at mechanical limits.  
@@ -273,6 +275,7 @@
 
 //Menu functions
 void debugging();
+void debug_trigger();
 void mainMenu();
 void caliMenu();
 void controlMenu();
@@ -311,6 +314,8 @@
     pwm_motor1.period(0.001);    
     pwm_motor2.period(0.001);
     
+    debugbtn.fall(&debug_trigger);
+    
     clearTerminal();            //Clear the putty window
     
     // make a menu, user has to initiate next step
@@ -399,10 +404,7 @@
                 emg_control=true;
                 control_method=2;
                 start_control(); wait(1);   
-                if(debug)
-                {
-                    debug_timer.attach(&debugging,1);
-                }                  
+             
                 controlButtons();
                 
                 break;        
@@ -444,15 +446,21 @@
 --------------------------------------------------------------------------------------------------------------------*/
 
 //Debug function: prints important variables to check if things are calculating correctly - find errors
+
+void debug_trigger(){
+    debug=!debug;
+}
+
 void debugging()
 {
+    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("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",q1_error,q2_error);
+    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);                
@@ -464,7 +472,7 @@
     pc.printf("Servopos us: %f \r\n",servopos);
     pc.printf("----------------------------------------\r\n");
     
-    
+    }
 }
 
 //Calculates how much servo needs to move to keep mouse aligned
@@ -486,6 +494,7 @@
 void controlButtons()
 {
     controlMenu();
+    debug_timer.attach(&debugging,1);
     char c=0;
     while(c != 'Q' && c != 'q') {
 
@@ -588,39 +597,7 @@
        
 }
 
-//Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit
-void shoulder()
-{
-    pwm_motor1=0;
-    done1 = true;
-    pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
-    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
-    theta1_cal = floor(theta1_lower * (4200/(2*PI)));
-    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses(int p)  
-}
 
-void elbow(){
-    pwm_motor2=0;
-    done2 = true;
-    pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
-    
-    //Mechanical limit 43 degrees -> 43*(4200/360) = 350
-    theta2_cal = floor(theta2_lower * (4200/(2*PI)));
-    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses(int p)    
-}
-
-//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
-            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
-            pc.printf("Motor 2 running %f \r\n");    
-        }   
-}
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
 void calibrate_arm(void)
@@ -666,75 +643,38 @@
   
 }
 
-//EMG Maximum Voluntary Contraction measurement
-void emg_mvc()
-{  
-        if(muscle==1){
-            
-            if(biceps_power>bicepsMVC){
-            //printf("+ ");
-            printf("%s+ %s",GREEN_,_GREEN);
-            bicepsMVC=biceps_power;
-            }    
-            else
-            printf("- ");
-        }  
-        
-        if(muscle==2){
-            
-            if(triceps_power>tricepsMVC){
-            printf("%s+ %s",GREEN_,_GREEN);
-            tricepsMVC=triceps_power;
-            }    
-            else
-            printf("- ");
-        }
-        
-        if(muscle==3){
-            
-            if(flexor_power>flexorMVC){
-            printf("%s+ %s",GREEN_,_GREEN);    
-            flexorMVC=flexor_power;
-            }    
-            else
-            printf("- ");
-        }
-        
-        if(muscle==4){
-            
-            if(extens_power>extensMVC){
-            printf("%s+ %s",GREEN_,_GREEN);    
-            extensMVC=extens_power;
-            }    
-            else
-            printf("- ");
-        }
-        
-    //}
-    calibrate_time = calibrate_time + 0.002;
-
+//Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit
+void shoulder()
+{
+    pwm_motor1=0;
+    done1 = true;
+    pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
+    //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
+    theta1_cal = floor(theta1_lower * (4200/(2*PI)));
+    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses(int p)  
 }
 
-void emg_min()
-{          
-            if(biceps_power>bicepsmin){
-            bicepsmin=biceps_power;
-            }    
-            
-            if(triceps_power>tricepsmin){
-            tricepsmin=triceps_power;
-            }            
+void elbow(){
+    pwm_motor2=0;
+    done2 = true;
+    pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
+    
+    //Mechanical limit 43 degrees -> 43*(4200/360) = 350
+    theta2_cal = floor(theta2_lower * (4200/(2*PI)));
+    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses(int p)    
+}
 
-            if(flexor_power>flexormin){
-            flexormin=flexor_power;
-            }            
- 
-            if(extens_power > extensmin){   
-            extensmin = extens_power;
-            }    
-        
-    calibrate_time = calibrate_time + 0.002;
-    
+//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
+            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
+            pc.printf("Motor 2 running %f \r\n");    
+        }   
 }
 
 //EMG calibration
@@ -874,6 +814,78 @@
 }
 
 
+//EMG Maximum Voluntary Contraction measurement
+void emg_mvc()
+{  
+        if(muscle==1){
+            
+            if(biceps_power>bicepsMVC){
+            //printf("+ ");
+            printf("%s+ %s",GREEN_,_GREEN);
+            bicepsMVC=biceps_power;
+            }    
+            else
+            printf("- ");
+        }  
+        
+        if(muscle==2){
+            
+            if(triceps_power>tricepsMVC){
+            printf("%s+ %s",GREEN_,_GREEN);
+            tricepsMVC=triceps_power;
+            }    
+            else
+            printf("- ");
+        }
+        
+        if(muscle==3){
+            
+            if(flexor_power>flexorMVC){
+            printf("%s+ %s",GREEN_,_GREEN);    
+            flexorMVC=flexor_power;
+            }    
+            else
+            printf("- ");
+        }
+        
+        if(muscle==4){
+            
+            if(extens_power>extensMVC){
+            printf("%s+ %s",GREEN_,_GREEN);    
+            extensMVC=extens_power;
+            }    
+            else
+            printf("- ");
+        }
+        
+    //}
+    calibrate_time = calibrate_time + 0.002;
+
+}
+
+void emg_min()
+{          
+            if(biceps_power>bicepsmin){
+            bicepsmin=biceps_power;
+            }    
+            
+            if(triceps_power>tricepsmin){
+            tricepsmin=triceps_power;
+            }            
+
+            if(flexor_power>flexormin){
+            flexormin=flexor_power;
+            }            
+ 
+            if(extens_power > extensmin){   
+            extensmin = extens_power;
+            }    
+        
+    calibrate_time = calibrate_time + 0.002;
+    
+}
+
+
 //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)
 {
@@ -1044,10 +1056,14 @@
     //inverse kinematics (error in position to error in angles)
     powlambda2 = pow(lambda,2);
     powlambda4 = pow(lambda,4);
-    dls1= -(l2*powlambda2*sin(theta1 + theta2) + l1*powlambda2*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))/(powlambda4 + 2*pow(l2,2)*powlambda2*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*powlambda2*pow(sin(theta1 + theta2),2) + pow(l1,2)*powlambda2*pow(cos(theta1),2) + pow(l1,2)*powlambda2*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*powlambda2*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*powlambda2*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls2= (l2*powlambda2*cos(theta1 + theta2) + l1*powlambda2*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))/(powlambda4 + 2*pow(l2,2)*powlambda2*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*powlambda2*pow(sin(theta1 + theta2),2) + pow(l1,2)*powlambda2*pow(cos(theta1),2) + pow(l1,2)*powlambda2*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*powlambda2*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*powlambda2*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls3= -(l2*powlambda2*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))/(powlambda4 + 2*pow(l2,2)*powlambda2*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*powlambda2*pow(sin(theta1 + theta2),2) + pow(l1,2)*powlambda2*pow(cos(theta1),2) + pow(l1,2)*powlambda2*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*powlambda2*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*powlambda2*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
-    dls4= (l2*powlambda2*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))/(powlambda4 + 2*pow(l2,2)*powlambda2*pow(cos(theta1 + theta2),2) + 2*pow(l2,2)*powlambda2*pow(sin(theta1 + theta2),2) + pow(l1,2)*powlambda2*pow(cos(theta1),2) + pow(l1,2)*powlambda2*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*powlambda2*cos(theta1 + theta2)*cos(theta1) + 2*l1*l2*powlambda2*sin(theta1 + theta2)*sin(theta1) - 2*pow(l1,2)*pow(l2,2)*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1));
+    powl2 = pow(l2,2);
+    powl1 = pow(l1,2);
+    sintheta1theta2 = sin(theta1 + theta2);
+    costheta1theta2 = cos(theta1 + theta2);
+    dls1= -(l2*powlambda2*sintheta1theta2 + l1*powlambda2*sin(theta1) + l1*powl2*pow(costheta1theta2,2)*sin(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
+    dls2= (l2*powlambda2*costheta1theta2 + l1*powlambda2*cos(theta1) + l1*powl2*pow(sintheta1theta2,2)*cos(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
+    dls3= -(l2*powlambda2*sintheta1theta2 - l1*powl2*pow(costheta1theta2,2)*sin(theta1) + powl1*l2*sintheta1theta2*pow(cos(theta1),2) - powl1*l2*costheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
+    dls4= (l2*powlambda2*costheta1theta2 - l1*powl2*pow(sintheta1theta2,2)*cos(theta1) + powl1*l2*costheta1theta2*pow(sin(theta1),2) - powl1*l2*sintheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
     
     q1_error = dls1 * x_error + dls2 * y_error;
     q2_error = dls3 * x_error + dls4 * y_error;