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@5:1190596b8842, 2015-05-25 (annotated)

Committer:
eencae
Date:
Mon May 25 14:14:23 2015 +0000
Revision:
5:1190596b8842
Parent:
0:74b4e50e0d15
Child:
6:a91d18a26ba3
Updated library so that the target is sent to the solve_inverse_kinematic() function which then returns the required angles in a vector struct. These angles are then passed to the move() function. This means the angles can now be stored in a LUT.

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