![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
Diff: kinematics.cpp
- Revision:
- 4:778bc352c47f
- Child:
- 5:01e1e68309ae
diff -r 10fa3102c2d7 -r 778bc352c47f kinematics.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kinematics.cpp Sun Oct 07 19:40:12 2018 +0000 @@ -0,0 +1,161 @@ + +#include "kinematics.h" +//this implementation has been taken from http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ +// robot geometry +// (look at pics above for explanation) +extern BufferedSerial buffered_pc; +Kinematics::Kinematics(Axis* A_, Axis* B_, Axis* C_,calVals calibration_) +{ + e = calibration_.e; + f = calibration_.f; + re = calibration_.re; + rf = calibration_.rf; + A = A_; + B = B_; + C = C_; + DeltaKinematics<float>::DeltaGeometricDim dim; + dim.sb = f; + dim.sp = e; + dim.L = rf; + dim.l = re; + dim.h = 60; + dim.max_neg_angle = -20; + dim.min_parallelogram_angle = 30; + + DK = new DeltaKinematics<float>(dim); + +} + + +// forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) +// returned status: 0=OK, -1=non-existing position +int Kinematics::delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) +{ + float t = (f-e)*tan30/2; + float dtr = pi/(float)180.0; + + theta1 *= dtr; + theta2 *= dtr; + theta3 *= dtr; + + float y1 = -(t + rf*cos(theta1)); + float z1 = -rf*sin(theta1); + + float y2 = (t + rf*cos(theta2))*sin30; + float x2 = y2*tan60; + float z2 = -rf*sin(theta2); + + float y3 = (t + rf*cos(theta3))*sin30; + float x3 = -y3*tan60; + float z3 = -rf*sin(theta3); + + float dnm = (y2-y1)*x3-(y3-y1)*x2; + + float w1 = y1*y1 + z1*z1; + float w2 = x2*x2 + y2*y2 + z2*z2; + float w3 = x3*x3 + y3*y3 + z3*z3; + + // x = (a1*z + b1)/dnm + float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); + float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; + + // y = (a2*z + b2)/dnm; + float a2 = -(z2-z1)*x3+(z3-z1)*x2; + float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; + + // a*z^2 + b*z + c = 0 + float a = a1*a1 + a2*a2 + dnm*dnm; + float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); + float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re); + + // discriminant + float d = b*b - (float)4.0*a*c; + if (d < 0) return -1; // non-existing point + + z0 = -(float)0.5*(b+sqrt(d))/a; + x0 = (a1*z0 + b1)/dnm; + y0 = (a2*z0 + b2)/dnm; + return 0; +} + +// inverse kinematics +// helper functions, calculates angle theta1 (for YZ-pane) +int Kinematics::delta_calcAngleYZ(float x0, float y0, float z0, float &theta) +{ + float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 + y0 -= 0.5 * 0.57735 * e; // shift center to edge + // z = a + b*y + float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0); + float b = (y1-y0)/z0; + // discriminant + float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); + if (d < 0) return -1; // non-existing point + float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point + float zj = a + b*yj; + theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); + return 0; +} + +// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) +// returned status: 0=OK, -1=non-existing position +int Kinematics::delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) +{ + theta1 = theta2 = theta3 = 0; + int status = delta_calcAngleYZ(x0, y0, z0, theta1); + if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg + if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to -120 deg + return status; +} + +void Kinematics::activateMotors() +{ + C->setMaxVel(15001); // for safety of the linkages, override to go fast! + Thread::wait(100); + A->setMaxVel(15001); + Thread::wait(100); + B->setMaxVel(15001); + Thread::wait(100); + C->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); + B->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); + A->odrive->serial_.printf("w axis0.controller.config.vel_limit %f\n",15002); + + int requested_state; + requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL; + A->runState(requested_state); + B->runState(requested_state); + C->runState(requested_state); +} + +void Kinematics::homeMotors() +{ + Thread::wait(1000); + A->homeAxis(); + Thread::wait(1000); + B->homeAxis(); + Thread::wait(1000); + C->homeAxis(); +} + +int Kinematics::goToPos(float x, float y, float z) +{ + //int error = delta_calcInverse(x,y,z,a,b,c); + DeltaKinematics<float>::DeltaVector DV = {x,y,z,0,0,0}; + int error = DK->CalculateIpk(&DV, 1); + //buffered_pc.printf("angles: a:%f , b:%f , c%f\r\n",DV.phi1,DV.phi2,DV.phi3); + DV.phi1 = (DV.phi1/180)*pi; + DV.phi2 = (DV.phi2/180)*pi; + DV.phi3 = (DV.phi3/180)*pi; + if(error == 0 ) { + A->goAngle(DV.phi1); + B->goAngle(DV.phi2); + C->goAngle(DV.phi3); + } + return error; +} + +void Kinematics::goToAngles(float a, float b, float c) +{ + A->goAngle(a); + B->goAngle(b); + C->goAngle(c); +} \ No newline at end of file