Diff: Leg.cpp

Revision:

0:74b4e50e0d15

Child:

5:1190596b8842

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Leg.cpp	Sun May 24 11:25:59 2015 +0000
@@ -0,0 +1,75 @@
+/*
+@file Leg.cpp
+
+@brief Member functions implementations
+
+*/
+#include "mbed.h"
+#include "Leg.h"
+
+Leg::Leg(PinName coxa_pin, PinName femur_pin, PinName tibia_pin)
+{
+    // set up pins as required
+    coxa_servo = new PwmOut(coxa_pin);
+    femur_servo = new PwmOut(femur_pin);
+    tibia_servo = new PwmOut(tibia_pin);
+    // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
+    // 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);
+}
+
+void Leg::solve_inverse_kinematics()
+{
+    // 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_);
+
+    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_;
+
+    //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_);
+}
+
+void Leg::move_to_target()
+{
+    // formulas to convert angle to pulse-width
+    // these need calibrating for each servo
+    // the base value is the pulse width that aligns the joint along the common normal
+    // 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;
+
+    //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse);
+    
+    // update servo pulse widths
+    coxa_servo->pulsewidth_us(int(coxa_pulse));
+    femur_servo->pulsewidth_us(int(femur_pulse));
+    tibia_servo->pulsewidth_us(int(tibia_pulse));
+
+}
+
+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;
+    femur_length_ = femur_length;
+    tibia_length_ = tibia_length;
+}
\ No newline at end of file