Library to control a 3DOF robotic leg. Specify an [x,y,z] co-ordinate and the library will solve the inverse kinematic equations to calculate the required servo pulse widths to position the foot at the target location.
Diff: Leg.cpp
- Revision:
- 5:1190596b8842
- Parent:
- 0:74b4e50e0d15
- Child:
- 6:a91d18a26ba3
diff -r e95f55af0d74 -r 1190596b8842 Leg.cpp --- a/Leg.cpp Mon May 25 13:53:41 2015 +0000 +++ b/Leg.cpp Mon May 25 14:14:23 2015 +0000 @@ -17,29 +17,34 @@ // can do it anyway to be sure. All channels share same period so just need to set one of them coxa_servo->period(1.0/50.0); // servos are typically 50 Hz // setup USB serial for debug/comms - serial = new Serial(USBTX,USBRX); + //serial = new Serial(USBTX,USBRX); } -void Leg::solve_inverse_kinematics() +vector_t Leg::solve_inverse_kinematics(float x, float y, float z) { // Equations from Quadrupedal Locomotion - An Introduction to Control of Four-legged Robot // Gonzalez de Santos, Garcia and Estremera, Springer-Verlag London, 2006 - coxa_angle_ = atan2(target_.y,target_.x); - - float A = -target_.z; - float E = target_.x*cos(coxa_angle_) + target_.y*sin(coxa_angle_); - float B = coxa_length_ - E; - float D = (2*coxa_length_*E + tibia_length_*tibia_length_ - femur_length_*femur_length_ - coxa_length_*coxa_length_ - target_.z*target_.z - E*E)/(2*femur_length_); + + vector_t angles; + + angles.x = atan2(y,x); // coxa - femur_angle_ = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D)); - tibia_angle_ = atan2(target_.z - femur_length_*sin(femur_angle_),E-femur_length_*cos(femur_angle_)-coxa_length_) - femur_angle_; + float A = -z; + float E = x*cos(angles.x) + y*sin(angles.x); + float B = coxa_length_ - E; + float D = (2*coxa_length_*E + tibia_length_*tibia_length_ - femur_length_*femur_length_ - coxa_length_*coxa_length_ - z*z - E*E)/(2*femur_length_); - //serial->printf("coxa_angle = %f\n",RAD2DEG*coxa_angle_); - //serial->printf("femur_angle = %f\n",RAD2DEG*femur_angle_); - //serial->printf("tibia_angle = %f\n",RAD2DEG*tibia_angle_); + angles.y = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D)); + angles.z = atan2(z - femur_length_*sin(angles.y),E-femur_length_*cos(angles.y)-coxa_length_) - angles.y; + + //serial->printf("coxa_angle = %f\n",RAD2DEG*angles.x); + //serial->printf("femur_angle = %f\n",RAD2DEG*angles.y); + //serial->printf("tibia_angle = %f\n",RAD2DEG*angles.z); + + return angles; } -void Leg::move_to_target() +void Leg::move(vector_t angles) { // formulas to convert angle to pulse-width // these need calibrating for each servo @@ -47,9 +52,9 @@ // the gradient value comes from measuring the pulse that moves the joint to 90 degree // i.e. gradient = (pulse_90 - pulse_normal)/90 // TODO implement calibration over serial - float coxa_pulse = 1510.0 + 10.44*coxa_angle_*RAD2DEG; - float femur_pulse = 1480.0 + 10.88*femur_angle_*RAD2DEG; - float tibia_pulse = 490.0 - 10.33*tibia_angle_*RAD2DEG; + float coxa_pulse = 1510.0 + 10.44*angles.x*RAD2DEG; + float femur_pulse = 1480.0 + 10.88*angles.y*RAD2DEG; + float tibia_pulse = 490.0 - 10.33*angles.z*RAD2DEG; //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse); @@ -60,13 +65,6 @@ } -void Leg::set_target(float x, float y, float z) -{ - target_.x = x; - target_.y = y; - target_.z = z; -} - void Leg::set_link_lengths(float coxa_length, float femur_length, float tibia_length) { coxa_length_ = coxa_length;