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.
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?
User | Revision | Line number | New 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 | } |