Diff: Leg.cpp

Revision:

5:1190596b8842

Parent:

0:74b4e50e0d15

Child:

6:a91d18a26ba3

--- a/Leg.cpp	Mon May 25 13:53:41 2015 +0000
+++ b/Leg.cpp	Mon May 25 14:14:23 2015 +0000
@@ -17,29 +17,34 @@
     // 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);
+    //serial = new Serial(USBTX,USBRX);
 }
 
-void Leg::solve_inverse_kinematics()
+vector_t Leg::solve_inverse_kinematics(float x, float y, float z)
 {
     // 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_);
+    
+    vector_t angles;
+    
+    angles.x = atan2(y,x);  // coxa
 
-    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_;
+    float A = -z;
+    float E = x*cos(angles.x) + y*sin(angles.x);
+    float B = coxa_length_ - E;
+    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_);
 
-    //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_);
+    angles.y = -atan2(B,A) + atan2(D,sqrt(A*A+B*B-D*D));
+    angles.z = atan2(z - femur_length_*sin(angles.y),E-femur_length_*cos(angles.y)-coxa_length_) - angles.y;
+
+    //serial->printf("coxa_angle = %f\n",RAD2DEG*angles.x);
+    //serial->printf("femur_angle = %f\n",RAD2DEG*angles.y);
+    //serial->printf("tibia_angle = %f\n",RAD2DEG*angles.z);
+    
+    return angles;
 }
 
-void Leg::move_to_target()
+void Leg::move(vector_t angles)
 {
     // formulas to convert angle to pulse-width
     // these need calibrating for each servo
@@ -47,9 +52,9 @@
     // 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;
+    float coxa_pulse = 1510.0 + 10.44*angles.x*RAD2DEG;
+    float femur_pulse = 1480.0 + 10.88*angles.y*RAD2DEG;
+    float tibia_pulse = 490.0 - 10.33*angles.z*RAD2DEG;
 
     //serial.printf("Servo Pulses (us): c = %f f =%f t = %f\n",coxa_pulse,femur_pulse,tibia_pulse);
     
@@ -60,13 +65,6 @@
 
 }
 
-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;