joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

kinematics.cpp@4:778bc352c47f, 2018-10-07 (annotated)

Committer:
je310
Date:
Sun Oct 07 19:40:12 2018 +0000
Revision:
4:778bc352c47f
Child:
5:01e1e68309ae
can work smoothly, though voltage reading not working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
je310 4:778bc352c47f 1
je310 4:778bc352c47f 2 #include "kinematics.h"
je310 4:778bc352c47f 3 //this implementation has been taken from http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
je310 4:778bc352c47f 4 // robot geometry
je310 4:778bc352c47f 5 // (look at pics above for explanation)
je310 4:778bc352c47f 6 extern BufferedSerial buffered_pc;
je310 4:778bc352c47f 7 Kinematics::Kinematics(Axis* A_, Axis* B_, Axis* C_,calVals calibration_)
je310 4:778bc352c47f 8 {
je310 4:778bc352c47f 9 e = calibration_.e;
je310 4:778bc352c47f 10 f = calibration_.f;
je310 4:778bc352c47f 11 re = calibration_.re;
je310 4:778bc352c47f 12 rf = calibration_.rf;
je310 4:778bc352c47f 13 A = A_;
je310 4:778bc352c47f 14 B = B_;
je310 4:778bc352c47f 15 C = C_;
je310 4:778bc352c47f 16 DeltaKinematics<float>::DeltaGeometricDim dim;
je310 4:778bc352c47f 17 dim.sb = f;
je310 4:778bc352c47f 18 dim.sp = e;
je310 4:778bc352c47f 19 dim.L = rf;
je310 4:778bc352c47f 20 dim.l = re;
je310 4:778bc352c47f 21 dim.h = 60;
je310 4:778bc352c47f 22 dim.max_neg_angle = -20;
je310 4:778bc352c47f 23 dim.min_parallelogram_angle = 30;
je310 4:778bc352c47f 24
je310 4:778bc352c47f 25 DK = new DeltaKinematics<float>(dim);
je310 4:778bc352c47f 26
je310 4:778bc352c47f 27 }
je310 4:778bc352c47f 28
je310 4:778bc352c47f 29
je310 4:778bc352c47f 30 // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
je310 4:778bc352c47f 31 // returned status: 0=OK, -1=non-existing position
je310 4:778bc352c47f 32 int Kinematics::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0)
je310 4:778bc352c47f 33 {
je310 4:778bc352c47f 34 float t = (f-e)*tan30/2;
je310 4:778bc352c47f 35 float dtr = pi/(float)180.0;
je310 4:778bc352c47f 36
je310 4:778bc352c47f 37 theta1 *= dtr;
je310 4:778bc352c47f 38 theta2 *= dtr;
je310 4:778bc352c47f 39 theta3 *= dtr;
je310 4:778bc352c47f 40
je310 4:778bc352c47f 41 float y1 = -(t + rf*cos(theta1));
je310 4:778bc352c47f 42 float z1 = -rf*sin(theta1);
je310 4:778bc352c47f 43
je310 4:778bc352c47f 44 float y2 = (t + rf*cos(theta2))*sin30;
je310 4:778bc352c47f 45 float x2 = y2*tan60;
je310 4:778bc352c47f 46 float z2 = -rf*sin(theta2);
je310 4:778bc352c47f 47
je310 4:778bc352c47f 48 float y3 = (t + rf*cos(theta3))*sin30;
je310 4:778bc352c47f 49 float x3 = -y3*tan60;
je310 4:778bc352c47f 50 float z3 = -rf*sin(theta3);
je310 4:778bc352c47f 51
je310 4:778bc352c47f 52 float dnm = (y2-y1)*x3-(y3-y1)*x2;
je310 4:778bc352c47f 53
je310 4:778bc352c47f 54 float w1 = y1*y1 + z1*z1;
je310 4:778bc352c47f 55 float w2 = x2*x2 + y2*y2 + z2*z2;
je310 4:778bc352c47f 56 float w3 = x3*x3 + y3*y3 + z3*z3;
je310 4:778bc352c47f 57
je310 4:778bc352c47f 58 // x = (a1*z + b1)/dnm
je310 4:778bc352c47f 59 float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
je310 4:778bc352c47f 60 float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
je310 4:778bc352c47f 61
je310 4:778bc352c47f 62 // y = (a2*z + b2)/dnm;
je310 4:778bc352c47f 63 float a2 = -(z2-z1)*x3+(z3-z1)*x2;
je310 4:778bc352c47f 64 float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
je310 4:778bc352c47f 65
je310 4:778bc352c47f 66 // a*z^2 + b*z + c = 0
je310 4:778bc352c47f 67 float a = a1*a1 + a2*a2 + dnm*dnm;
je310 4:778bc352c47f 68 float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
je310 4:778bc352c47f 69 float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
je310 4:778bc352c47f 70
je310 4:778bc352c47f 71 // discriminant
je310 4:778bc352c47f 72 float d = b*b - (float)4.0*a*c;
je310 4:778bc352c47f 73 if (d < 0) return -1; // non-existing point
je310 4:778bc352c47f 74
je310 4:778bc352c47f 75 z0 = -(float)0.5*(b+sqrt(d))/a;
je310 4:778bc352c47f 76 x0 = (a1*z0 + b1)/dnm;
je310 4:778bc352c47f 77 y0 = (a2*z0 + b2)/dnm;
je310 4:778bc352c47f 78 return 0;
je310 4:778bc352c47f 79 }
je310 4:778bc352c47f 80
je310 4:778bc352c47f 81 // inverse kinematics
je310 4:778bc352c47f 82 // helper functions, calculates angle theta1 (for YZ-pane)
je310 4:778bc352c47f 83 int Kinematics::delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
je310 4:778bc352c47f 84 {
je310 4:778bc352c47f 85 float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
je310 4:778bc352c47f 86 y0 -= 0.5 * 0.57735 * e; // shift center to edge
je310 4:778bc352c47f 87 // z = a + b*y
je310 4:778bc352c47f 88 float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
je310 4:778bc352c47f 89 float b = (y1-y0)/z0;
je310 4:778bc352c47f 90 // discriminant
je310 4:778bc352c47f 91 float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
je310 4:778bc352c47f 92 if (d < 0) return -1; // non-existing point
je310 4:778bc352c47f 93 float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
je310 4:778bc352c47f 94 float zj = a + b*yj;
je310 4:778bc352c47f 95 theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
je310 4:778bc352c47f 96 return 0;
je310 4:778bc352c47f 97 }
je310 4:778bc352c47f 98
je310 4:778bc352c47f 99 // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
je310 4:778bc352c47f 100 // returned status: 0=OK, -1=non-existing position
je310 4:778bc352c47f 101 int Kinematics::delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3)
je310 4:778bc352c47f 102 {
je310 4:778bc352c47f 103 theta1 = theta2 = theta3 = 0;
je310 4:778bc352c47f 104 int status = delta_calcAngleYZ(x0, y0, z0, theta1);
je310 4:778bc352c47f 105 if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg
je310 4:778bc352c47f 106 if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg
je310 4:778bc352c47f 107 return status;
je310 4:778bc352c47f 108 }
je310 4:778bc352c47f 109
je310 4:778bc352c47f 110 void Kinematics::activateMotors()
je310 4:778bc352c47f 111 {
je310 4:778bc352c47f 112 C->setMaxVel(15001); // for safety of the linkages, override to go fast!
je310 4:778bc352c47f 113 Thread::wait(100);
je310 4:778bc352c47f 114 A->setMaxVel(15001);
je310 4:778bc352c47f 115 Thread::wait(100);
je310 4:778bc352c47f 116 B->setMaxVel(15001);
je310 4:778bc352c47f 117 Thread::wait(100);
je310 4:778bc352c47f 118 C->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
je310 4:778bc352c47f 119 B->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
je310 4:778bc352c47f 120 A->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002);
je310 4:778bc352c47f 121
je310 4:778bc352c47f 122 int requested_state;
je310 4:778bc352c47f 123 requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
je310 4:778bc352c47f 124 A->runState(requested_state);
je310 4:778bc352c47f 125 B->runState(requested_state);
je310 4:778bc352c47f 126 C->runState(requested_state);
je310 4:778bc352c47f 127 }
je310 4:778bc352c47f 128
je310 4:778bc352c47f 129 void Kinematics::homeMotors()
je310 4:778bc352c47f 130 {
je310 4:778bc352c47f 131 Thread::wait(1000);
je310 4:778bc352c47f 132 A->homeAxis();
je310 4:778bc352c47f 133 Thread::wait(1000);
je310 4:778bc352c47f 134 B->homeAxis();
je310 4:778bc352c47f 135 Thread::wait(1000);
je310 4:778bc352c47f 136 C->homeAxis();
je310 4:778bc352c47f 137 }
je310 4:778bc352c47f 138
je310 4:778bc352c47f 139 int Kinematics::goToPos(float x, float y, float z)
je310 4:778bc352c47f 140 {
je310 4:778bc352c47f 141 //int error = delta_calcInverse(x,y,z,a,b,c);
je310 4:778bc352c47f 142 DeltaKinematics<float>::DeltaVector DV = {x,y,z,0,0,0};
je310 4:778bc352c47f 143 int error = DK->CalculateIpk(&DV, 1);
je310 4:778bc352c47f 144 //buffered_pc.printf("angles: a:%f , b:%f , c%f\r\n",DV.phi1,DV.phi2,DV.phi3);
je310 4:778bc352c47f 145 DV.phi1 = (DV.phi1/180)*pi;
je310 4:778bc352c47f 146 DV.phi2 = (DV.phi2/180)*pi;
je310 4:778bc352c47f 147 DV.phi3 = (DV.phi3/180)*pi;
je310 4:778bc352c47f 148 if(error == 0 ) {
je310 4:778bc352c47f 149 A->goAngle(DV.phi1);
je310 4:778bc352c47f 150 B->goAngle(DV.phi2);
je310 4:778bc352c47f 151 C->goAngle(DV.phi3);
je310 4:778bc352c47f 152 }
je310 4:778bc352c47f 153 return error;
je310 4:778bc352c47f 154 }
je310 4:778bc352c47f 155
je310 4:778bc352c47f 156 void Kinematics::goToAngles(float a, float b, float c)
je310 4:778bc352c47f 157 {
je310 4:778bc352c47f 158 A->goAngle(a);
je310 4:778bc352c47f 159 B->goAngle(b);
je310 4:778bc352c47f 160 C->goAngle(c);
je310 4:778bc352c47f 161 }