Biorobotics / Mbed 2 deprecated frdm_kinematics

Direct and inverse kinematics of a scara robotic arm, based on trigonometry

Dependencies:   MODSERIAL mbed

main.cpp@4:022b69da83f8, 2015-10-19 (annotated)

Committer:
apdijkshoorn
Date:
Mon Oct 19 13:25:24 2015 +0000
Revision:
4:022b69da83f8
Parent:
3:1e8cfc385b48 
Functions for direct and inverse kinematics of a scara robotic arm, based on trigonometry

Who changed what in which revision?

UserRevisionLine numberNew 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 3:1e8cfc385b48 12 double theta2_0 = 180;
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 3:1e8cfc385b48 27 double L2 = 0.26;
apdijkshoorn 3:1e8cfc385b48 28 double theta1_0 = 30;
apdijkshoorn 4:022b69da83f8 29 double theta2_0 = 180;
apdijkshoorn 0:8d4438eabc82 30
apdijkshoorn 2:47913550ec8d 31 double theta_2 = acos( (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2) )* 180 / PI;
apdijkshoorn 0:8d4438eabc82 32 double theta_total = atan2 (y,x) * 180 / PI;
apdijkshoorn 1:dd8fca950fed 33 double theta_Q = acos( (x*x+y*y+L1*L1-L2*L2) / (2*L1*sqrt(x*x+y*y)) )* 180 / PI;
apdijkshoorn 0:8d4438eabc82 34 double theta_1 = theta_total - theta_Q;
apdijkshoorn 3:1e8cfc385b48 35
apdijkshoorn 3:1e8cfc385b48 36 a = theta_1 - theta1_0;
apdijkshoorn 3:1e8cfc385b48 37 b = theta_2 - theta2_0;
apdijkshoorn 0:8d4438eabc82 38 }
apdijkshoorn 0:8d4438eabc82 39
apdijkshoorn 0:8d4438eabc82 40
apdijkshoorn 0:8d4438eabc82 41 int main()
apdijkshoorn 0:8d4438eabc82 42 {
apdijkshoorn 0:8d4438eabc82 43 pc.baud(115200);
apdijkshoorn 0:8d4438eabc82 44
apdijkshoorn 3:1e8cfc385b48 45 double theta_1 = 0.000001;
apdijkshoorn 3:1e8cfc385b48 46 double theta_2 = 0.000001;
apdijkshoorn 0:8d4438eabc82 47
apdijkshoorn 0:8d4438eabc82 48 double xy[2] = {0.0,0.0};
apdijkshoorn 0:8d4438eabc82 49
apdijkshoorn 0:8d4438eabc82 50
apdijkshoorn 0:8d4438eabc82 51
apdijkshoorn 0:8d4438eabc82 52 while (true) {
apdijkshoorn 0:8d4438eabc82 53 wait(0.5f);
apdijkshoorn 0:8d4438eabc82 54 DirectKin(xy[0],xy[1], theta_1, theta_2);
apdijkshoorn 3:1e8cfc385b48 55 pc.printf("%4.4f %4.4f \n",xy[0], xy[1]);
apdijkshoorn 3:1e8cfc385b48 56
apdijkshoorn 0:8d4438eabc82 57 IndirectKin(theta_1, theta_2, xy[0], xy[1]);
apdijkshoorn 3:1e8cfc385b48 58 pc.printf("%4.4f %4.4f \n\n",theta_1, theta_2);
apdijkshoorn 3:1e8cfc385b48 59
apdijkshoorn 0:8d4438eabc82 60 theta_1 = theta_1 + 90.0;
apdijkshoorn 0:8d4438eabc82 61 }
apdijkshoorn 0:8d4438eabc82 62 }