![](/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:
- 33:daa6ec305441
- Parent:
- 32:76c4d7bb2022
- Child:
- 34:d6ec7c634763
diff -r 76c4d7bb2022 -r daa6ec305441 main.cpp --- a/main.cpp Thu Oct 22 10:57:50 2015 +0000 +++ b/main.cpp Thu Oct 22 11:03:51 2015 +0000 @@ -902,8 +902,8 @@ //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)); /* - dls2= (l2*pow(lambda,2)*cos(theta1 + theta2) + l1*pow(lambda,2)*cos(theta1) + l1*pow(l2,2)*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*l2^2*pow(lambda,2)*pow(sin(theta1 + theta2),2) + pow(l1,2)*pow(lambda,2)*cos(theta1)^2 + pow(l1,2)*pow(lambda,2)*pow(sin(theta1),2) + pow(l1,2)*pow(l2,2)*pow(cos(theta1 + theta2),2)*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*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*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); dls3= -(l2*pow(lambda,2)*sin(theta1 + theta2) - l1*pow(l2,2)*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)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*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*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); dls4= (l2*pow(lambda,2)*cos(theta1 + theta2) - l1*pow(l2,2)*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)*cos(theta1)^2 + l1^2*lambda^2*sin(theta1)^2 + l1^2*l2^2*cos(theta1 + theta2)^2*sin(theta1)^2 + l1^2*l2^2*sin(theta1 + theta2)^2*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*l1^2*l2^2*cos(theta1 + theta2)*sin(theta1 + theta2)*cos(theta1)*sin(theta1)); */