Craig Evans / Leg

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.

Dependencies:   Utils

Leg.cpp@0:74b4e50e0d15, 2015-05-24 (annotated)

Committer:
eencae
Date:
Sun May 24 11:25:59 2015 +0000
Revision:
0:74b4e50e0d15
Child:
5:1190596b8842
Initial commit.; ; Library to control a 3DOF robotic leg. Solves IK and is able to position foot at specified [x,y,z] coordinate.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:74b4e50e0d15 1 /*
eencae 0:74b4e50e0d15 2 @file Leg.cpp
eencae 0:74b4e50e0d15 3
eencae 0:74b4e50e0d15 4 @brief Member functions implementations
eencae 0:74b4e50e0d15 5
eencae 0:74b4e50e0d15 6 */
eencae 0:74b4e50e0d15 7 #include "mbed.h"
eencae 0:74b4e50e0d15 8 #include "Leg.h"
eencae 0:74b4e50e0d15 9
eencae 0:74b4e50e0d15 10 Leg::Leg(PinName coxa_pin, PinName femur_pin, PinName tibia_pin)
eencae 0:74b4e50e0d15 11 {
eencae 0:74b4e50e0d15 12 // set up pins as required
eencae 0:74b4e50e0d15 13 coxa_servo = new PwmOut(coxa_pin);
eencae 0:74b4e50e0d15 14 femur_servo = new PwmOut(femur_pin);
eencae 0:74b4e50e0d15 15 tibia_servo = new PwmOut(tibia_pin);
eencae 0:74b4e50e0d15 16 // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
eencae 0:74b4e50e0d15 17 // can do it anyway to be sure. All channels share same period so just need to set one of them
eencae 0:74b4e50e0d15 18 coxa_servo->period(1.0/50.0); // servos are typically 50 Hz
eencae 0:74b4e50e0d15 19 // setup USB serial for debug/comms
eencae 0:74b4e50e0d15 20 serial = new Serial(USBTX,USBRX);
eencae 0:74b4e50e0d15 21 }
eencae 0:74b4e50e0d15 22
eencae 0:74b4e50e0d15 23 void Leg::solve_inverse_kinematics()
eencae 0:74b4e50e0d15 24 {
eencae 0:74b4e50e0d15 25 // Equations from Quadrupedal Locomotion - An Introduction to Control of Four-legged Robot
eencae 0:74b4e50e0d15 26 // Gonzalez de Santos, Garcia and Estremera, Springer-Verlag London, 2006
eencae 0:74b4e50e0d15 27 coxa_angle_ = atan2(target_.y,target_.x);
eencae 0:74b4e50e0d15 28
eencae 0:74b4e50e0d15 29 float A = -target_.z;
eencae 0:74b4e50e0d15 30 float E = target_.x*cos(coxa_angle_) + target_.y*sin(coxa_angle_);
eencae 0:74b4e50e0d15 31 float B = coxa_length_ - E;
eencae 0:74b4e50e0d15 32 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_);
eencae 0:74b4e50e0d15 33
eencae 0:74b4e50e0d15 34 femur_angle_ = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D));
eencae 0:74b4e50e0d15 35 tibia_angle_ = atan2(target_.z - femur_length_*sin(femur_angle_),E-femur_length_*cos(femur_angle_)-coxa_length_) - femur_angle_;
eencae 0:74b4e50e0d15 36
eencae 0:74b4e50e0d15 37 //serial->printf("coxa_angle = %f\n",RAD2DEG*coxa_angle_);
eencae 0:74b4e50e0d15 38 //serial->printf("femur_angle = %f\n",RAD2DEG*femur_angle_);
eencae 0:74b4e50e0d15 39 //serial->printf("tibia_angle = %f\n",RAD2DEG*tibia_angle_);
eencae 0:74b4e50e0d15 40 }
eencae 0:74b4e50e0d15 41
eencae 0:74b4e50e0d15 42 void Leg::move_to_target()
eencae 0:74b4e50e0d15 43 {
eencae 0:74b4e50e0d15 44 // formulas to convert angle to pulse-width
eencae 0:74b4e50e0d15 45 // these need calibrating for each servo
eencae 0:74b4e50e0d15 46 // the base value is the pulse width that aligns the joint along the common normal
eencae 0:74b4e50e0d15 47 // the gradient value comes from measuring the pulse that moves the joint to 90 degree
eencae 0:74b4e50e0d15 48 // i.e. gradient = (pulse_90 - pulse_normal)/90
eencae 0:74b4e50e0d15 49 // TODO implement calibration over serial
eencae 0:74b4e50e0d15 50 float coxa_pulse = 1510.0 + 10.44*coxa_angle_*RAD2DEG;
eencae 0:74b4e50e0d15 51 float femur_pulse = 1480.0 + 10.88*femur_angle_*RAD2DEG;
eencae 0:74b4e50e0d15 52 float tibia_pulse = 490.0 - 10.33*tibia_angle_*RAD2DEG;
eencae 0:74b4e50e0d15 53
eencae 0:74b4e50e0d15 54 //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse);
eencae 0:74b4e50e0d15 55
eencae 0:74b4e50e0d15 56 // update servo pulse widths
eencae 0:74b4e50e0d15 57 coxa_servo->pulsewidth_us(int(coxa_pulse));
eencae 0:74b4e50e0d15 58 femur_servo->pulsewidth_us(int(femur_pulse));
eencae 0:74b4e50e0d15 59 tibia_servo->pulsewidth_us(int(tibia_pulse));
eencae 0:74b4e50e0d15 60
eencae 0:74b4e50e0d15 61 }
eencae 0:74b4e50e0d15 62
eencae 0:74b4e50e0d15 63 void Leg::set_target(float x, float y, float z)
eencae 0:74b4e50e0d15 64 {
eencae 0:74b4e50e0d15 65 target_.x = x;
eencae 0:74b4e50e0d15 66 target_.y = y;
eencae 0:74b4e50e0d15 67 target_.z = z;
eencae 0:74b4e50e0d15 68 }
eencae 0:74b4e50e0d15 69
eencae 0:74b4e50e0d15 70 void Leg::set_link_lengths(float coxa_length, float femur_length, float tibia_length)
eencae 0:74b4e50e0d15 71 {
eencae 0:74b4e50e0d15 72 coxa_length_ = coxa_length;
eencae 0:74b4e50e0d15 73 femur_length_ = femur_length;
eencae 0:74b4e50e0d15 74 tibia_length_ = tibia_length;
eencae 0:74b4e50e0d15 75 }