![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 57:d6192801fd6d
- Parent:
- 56:5ff9e5c1ed44
- Child:
- 58:db11481da856
--- 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;