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@6:a91d18a26ba3, 2015-05-25 (annotated)

Committer:
eencae
Date:
Mon May 25 15:04:57 2015 +0000
Revision:
6:a91d18a26ba3
Parent:
5:1190596b8842 
Added default robotic leg dimensions.

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 5:1190596b8842 20 //serial = new Serial(USBTX,USBRX);
eencae 6:a91d18a26ba3 21
eencae 6:a91d18a26ba3 22 coxa_length_ = 28.0;
eencae 6:a91d18a26ba3 23 femur_length_ = 58.0;
eencae 6:a91d18a26ba3 24 tibia_length_ = 125.0;
eencae 6:a91d18a26ba3 25
eencae 0:74b4e50e0d15 26 }
eencae 0:74b4e50e0d15 27
eencae 5:1190596b8842 28 vector_t Leg::solve_inverse_kinematics(float x, float y, float z)
eencae 0:74b4e50e0d15 29 {
eencae 0:74b4e50e0d15 30 // Equations from Quadrupedal Locomotion - An Introduction to Control of Four-legged Robot
eencae 0:74b4e50e0d15 31 // Gonzalez de Santos, Garcia and Estremera, Springer-Verlag London, 2006
eencae 5:1190596b8842 32
eencae 5:1190596b8842 33 vector_t angles;
eencae 5:1190596b8842 34
eencae 5:1190596b8842 35 angles.x = atan2(y,x); // coxa
eencae 0:74b4e50e0d15 36
eencae 5:1190596b8842 37 float A = -z;
eencae 5:1190596b8842 38 float E = x*cos(angles.x) + y*sin(angles.x);
eencae 5:1190596b8842 39 float B = coxa_length_ - E;
eencae 5:1190596b8842 40 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_);
eencae 0:74b4e50e0d15 41
eencae 5:1190596b8842 42 angles.y = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D));
eencae 5:1190596b8842 43 angles.z = atan2(z - femur_length_*sin(angles.y),E-femur_length_*cos(angles.y)-coxa_length_) - angles.y;
eencae 5:1190596b8842 44
eencae 5:1190596b8842 45 //serial->printf("coxa_angle = %f\n",RAD2DEG*angles.x);
eencae 5:1190596b8842 46 //serial->printf("femur_angle = %f\n",RAD2DEG*angles.y);
eencae 5:1190596b8842 47 //serial->printf("tibia_angle = %f\n",RAD2DEG*angles.z);
eencae 5:1190596b8842 48
eencae 5:1190596b8842 49 return angles;
eencae 0:74b4e50e0d15 50 }
eencae 0:74b4e50e0d15 51
eencae 5:1190596b8842 52 void Leg::move(vector_t angles)
eencae 0:74b4e50e0d15 53 {
eencae 0:74b4e50e0d15 54 // formulas to convert angle to pulse-width
eencae 0:74b4e50e0d15 55 // these need calibrating for each servo
eencae 0:74b4e50e0d15 56 // the base value is the pulse width that aligns the joint along the common normal
eencae 0:74b4e50e0d15 57 // the gradient value comes from measuring the pulse that moves the joint to 90 degree
eencae 0:74b4e50e0d15 58 // i.e. gradient = (pulse_90 - pulse_normal)/90
eencae 0:74b4e50e0d15 59 // TODO implement calibration over serial
eencae 5:1190596b8842 60 float coxa_pulse = 1510.0 + 10.44*angles.x*RAD2DEG;
eencae 5:1190596b8842 61 float femur_pulse = 1480.0 + 10.88*angles.y*RAD2DEG;
eencae 5:1190596b8842 62 float tibia_pulse = 490.0 - 10.33*angles.z*RAD2DEG;
eencae 0:74b4e50e0d15 63
eencae 0:74b4e50e0d15 64 //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse);
eencae 0:74b4e50e0d15 65
eencae 0:74b4e50e0d15 66 // update servo pulse widths
eencae 0:74b4e50e0d15 67 coxa_servo->pulsewidth_us(int(coxa_pulse));
eencae 0:74b4e50e0d15 68 femur_servo->pulsewidth_us(int(femur_pulse));
eencae 0:74b4e50e0d15 69 tibia_servo->pulsewidth_us(int(tibia_pulse));
eencae 0:74b4e50e0d15 70
eencae 0:74b4e50e0d15 71 }
eencae 0:74b4e50e0d15 72
eencae 0:74b4e50e0d15 73 void Leg::set_link_lengths(float coxa_length, float femur_length, float tibia_length)
eencae 0:74b4e50e0d15 74 {
eencae 0:74b4e50e0d15 75 coxa_length_ = coxa_length;
eencae 0:74b4e50e0d15 76 femur_length_ = femur_length;
eencae 0:74b4e50e0d15 77 tibia_length_ = tibia_length;
eencae 0:74b4e50e0d15 78 }