Diff: main.cpp

Revision:

57:d6192801fd6d

Parent:

56:5ff9e5c1ed44

Child:

58:db11481da856

diff -r 5ff9e5c1ed44 -r d6192801fd6d main.cpp
--- a/main.cpp	Tue Oct 27 19:56:59 2015 +0000
+++ b/main.cpp	Tue Oct 27 20:46:20 2015 +0000
@@ -14,7 +14,7 @@
 //Define important constants in memory
 #define     PI              3.14159265
 #define     SAMPLE_RATE     0.002   //500 Hz EMG sample rate
-#define     CONTROL_RATE    0.005   //100 Hz Control rate
+#define     CONTROL_RATE    0.01   //100 Hz Control rate
 #define     SERVO_RATE      0.05    //50 Hz Servo Control rate
 #define     ENCODER_CPR     4200    //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
                                     //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
@@ -40,7 +40,7 @@
 Ticker      control_timer;      //Ticker for control loop
 Ticker      servo_timer;        //Ticker for servo control
 Ticker      debug_timer;
-HIDScope    scope(4);           //Scope 4 channels
+//HIDScope    scope(2);           //Scope 4 channels
 
 // AnalogIn potmeter(A4);       //potmeters
 // AnalogIn potmeter2(A5);      //
@@ -199,6 +199,7 @@
 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
@@ -448,9 +449,9 @@
     //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("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("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("PID output: %f and %f \r\n",u1,u2);
     pc.printf("----------------------------------------\r\n");
@@ -1041,10 +1042,12 @@
     /******************************** 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));
-    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));
+    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));
     
     q1_error = dls1 * x_error + dls2 * y_error;
     q2_error = dls3 * x_error + dls4 * y_error;