Biorobotics
/
frdm_kinematics
Direct and inverse kinematics of a scara robotic arm, based on trigonometry
main.cpp@0:8d4438eabc82, 2015-10-19 (annotated)
- Committer:
- apdijkshoorn
- Date:
- Mon Oct 19 12:33:06 2015 +0000
- Revision:
- 0:8d4438eabc82
- Child:
- 1:dd8fca950fed
Debugging indirect kinematics function
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
apdijkshoorn | 0:8d4438eabc82 | 1 | #include "mbed.h" |
apdijkshoorn | 0:8d4438eabc82 | 2 | #include "MODSERIAL.h" |
apdijkshoorn | 0:8d4438eabc82 | 3 | #define PI 3.14159265 |
apdijkshoorn | 0:8d4438eabc82 | 4 | |
apdijkshoorn | 0:8d4438eabc82 | 5 | MODSERIAL pc(USBTX, USBRX); |
apdijkshoorn | 0:8d4438eabc82 | 6 | |
apdijkshoorn | 0:8d4438eabc82 | 7 | void DirectKin(double& a, double& b, double theta1, double theta2) |
apdijkshoorn | 0:8d4438eabc82 | 8 | { |
apdijkshoorn | 0:8d4438eabc82 | 9 | double L1 = 0.34; |
apdijkshoorn | 0:8d4438eabc82 | 10 | double L2 = 0.26; |
apdijkshoorn | 0:8d4438eabc82 | 11 | double theta1_0 = 0; |
apdijkshoorn | 0:8d4438eabc82 | 12 | double theta2_0 = 0; |
apdijkshoorn | 0:8d4438eabc82 | 13 | |
apdijkshoorn | 0:8d4438eabc82 | 14 | double r0_1[2] = {L1*cos((theta1_0+theta1) * PI / 180.0) , L1*sin((theta1_0+theta1) * PI / 180.0)}; |
apdijkshoorn | 0:8d4438eabc82 | 15 | double r0_2[2] = {L1*cos((theta1_0+theta1) * PI / 180.0)+L2*cos((theta1_0+theta1+theta2+theta2_0)* PI / 180.0), L1*sin((theta1_0+theta1) * PI / 180.0)+L2*sin((theta1_0+theta1+theta2+theta2_0)* PI / 180.0)}; |
apdijkshoorn | 0:8d4438eabc82 | 16 | |
apdijkshoorn | 0:8d4438eabc82 | 17 | double x = r0_2[0]; |
apdijkshoorn | 0:8d4438eabc82 | 18 | double y = r0_2[1]; |
apdijkshoorn | 0:8d4438eabc82 | 19 | |
apdijkshoorn | 0:8d4438eabc82 | 20 | a = x; |
apdijkshoorn | 0:8d4438eabc82 | 21 | b = y; |
apdijkshoorn | 0:8d4438eabc82 | 22 | } |
apdijkshoorn | 0:8d4438eabc82 | 23 | |
apdijkshoorn | 0:8d4438eabc82 | 24 | void IndirectKin(double&a, double& b, double x, double y) |
apdijkshoorn | 0:8d4438eabc82 | 25 | { |
apdijkshoorn | 0:8d4438eabc82 | 26 | double L1 = 0.34; |
apdijkshoorn | 0:8d4438eabc82 | 27 | double L2 = 0.26; |
apdijkshoorn | 0:8d4438eabc82 | 28 | double para2 = (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2); |
apdijkshoorn | 0:8d4438eabc82 | 29 | |
apdijkshoorn | 0:8d4438eabc82 | 30 | //double theta_1 = 0.0; |
apdijkshoorn | 0:8d4438eabc82 | 31 | double theta_2 = 0.0; |
apdijkshoorn | 0:8d4438eabc82 | 32 | double theta_Q = 0.0; |
apdijkshoorn | 0:8d4438eabc82 | 33 | |
apdijkshoorn | 0:8d4438eabc82 | 34 | //double theta_2 = acos(para2)* 180 / PI; |
apdijkshoorn | 0:8d4438eabc82 | 35 | double theta_total = atan2 (y,x) * 180 / PI; |
apdijkshoorn | 0:8d4438eabc82 | 36 | //double theta_Q = acos( (x*x+y*y+L1*L1-L2*L2) / (2*L1*sqrt(x*x+y*y)) )* 180 / PI; |
apdijkshoorn | 0:8d4438eabc82 | 37 | double theta_1 = theta_total - theta_Q; |
apdijkshoorn | 0:8d4438eabc82 | 38 | |
apdijkshoorn | 0:8d4438eabc82 | 39 | a = theta_1; |
apdijkshoorn | 0:8d4438eabc82 | 40 | b = theta_2; |
apdijkshoorn | 0:8d4438eabc82 | 41 | //a = theta_1; |
apdijkshoorn | 0:8d4438eabc82 | 42 | //b = theta_2; |
apdijkshoorn | 0:8d4438eabc82 | 43 | } |
apdijkshoorn | 0:8d4438eabc82 | 44 | |
apdijkshoorn | 0:8d4438eabc82 | 45 | |
apdijkshoorn | 0:8d4438eabc82 | 46 | int main() |
apdijkshoorn | 0:8d4438eabc82 | 47 | { |
apdijkshoorn | 0:8d4438eabc82 | 48 | pc.baud(115200); |
apdijkshoorn | 0:8d4438eabc82 | 49 | |
apdijkshoorn | 0:8d4438eabc82 | 50 | double theta_1 = 0.0; |
apdijkshoorn | 0:8d4438eabc82 | 51 | double theta_2 = 0.0; |
apdijkshoorn | 0:8d4438eabc82 | 52 | |
apdijkshoorn | 0:8d4438eabc82 | 53 | double xy[2] = {0.0,0.0}; |
apdijkshoorn | 0:8d4438eabc82 | 54 | |
apdijkshoorn | 0:8d4438eabc82 | 55 | |
apdijkshoorn | 0:8d4438eabc82 | 56 | |
apdijkshoorn | 0:8d4438eabc82 | 57 | while (true) { |
apdijkshoorn | 0:8d4438eabc82 | 58 | wait(0.5f); |
apdijkshoorn | 0:8d4438eabc82 | 59 | DirectKin(xy[0],xy[1], theta_1, theta_2); |
apdijkshoorn | 0:8d4438eabc82 | 60 | // pc.printf("%4.4f %4.4f \n",xy[0], xy[1]); |
apdijkshoorn | 0:8d4438eabc82 | 61 | |
apdijkshoorn | 0:8d4438eabc82 | 62 | IndirectKin(theta_1, theta_2, xy[0], xy[1]); |
apdijkshoorn | 0:8d4438eabc82 | 63 | pc.printf("%4.4f %4.4f \n",theta_1, theta_2); |
apdijkshoorn | 0:8d4438eabc82 | 64 | |
apdijkshoorn | 0:8d4438eabc82 | 65 | theta_1 = theta_1 + 90.0; |
apdijkshoorn | 0:8d4438eabc82 | 66 | } |
apdijkshoorn | 0:8d4438eabc82 | 67 | } |