Mercurial > tote

changeset 158:4286f9773694 teensy31

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Use float functions everywhere
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Wed, 01 Jul 2015 12:51:53 +0200
parents 7f7811e22e63
children 806a193584f9
files start/creep.ino start/leg.ino start/misc.h start/misc.ino
diffstat 4 files changed, 35 insertions(+), 38 deletions(-) [+]
line wrap: on
line diff
--- a/start/creep.ino	Tue Jun 30 18:05:23 2015 +0200
+++ b/start/creep.ino	Wed Jul 01 12:51:53 2015 +0200
@@ -68,7 +68,7 @@
 
 /* Shifts the whole body by defined amount. */
 void _shift_body_by(float dx, float dy) {
-    for (unsigned char leg=0; leg < 4; ++leg) {
+    for (unsigned char leg = 0; leg < 4; ++leg) {
         move_leg_by(leg, dx * LEG_X[leg], dy * LEG_Y[leg], 0);
     }
     _shift_x += dx;
@@ -149,21 +149,21 @@
 
 unsigned char step_order() {
     // Decide on the best leg order.
-    if (abs(creep_dy) > abs(creep_dx)) {
+    if (max(creep_dy, -creep_dy) > max(creep_dx, -creep_dx)) {
         // forward/backward
         if (creep_dy > 0) {
             return 0; // forward
         } else {
             return 1; // backward
         }
-    } else if (abs(creep_dx) > abs(creep_dy)) {
+    } else if (max(creep_dx, -creep_dx) > max(creep_dy, -creep_dy)) {
         // sideways
         if (creep_dx < 0) {
             return 4; // left
         } else {
             return 5; // right
         }
-    } else if (abs(creep_rotation) > 0.001) {
+    } else if (max(creep_rotation, -creep_rotation) > 0.001) {
         // rotate
         if (creep_rotation > 0) {
             return 2; // clockwise
@@ -177,8 +177,9 @@
 /* Perform one step of a walk, selected depending on the speed. */
 void walk() {
     static unsigned char leg = 0;
-    float max_speed = max(abs(creep_rotation),
-                           max(abs(creep_dx), abs(creep_dy)));
+    float max_speed = max(max(creep_rotation, -creep_rotation),
+                          max(max(creep_dx, -creep_dx),
+                              max(creep_dy, -creep_dy)));
 
     if (max_speed > 1.5) {
         _trot_step(leg);
--- a/start/leg.ino	Tue Jun 30 18:05:23 2015 +0200
+++ b/start/leg.ino	Wed Jul 01 12:51:53 2015 +0200
@@ -18,16 +18,16 @@
 
 float _solve_triangle(float a, float b, float c) {
     // Calculate the angle between a and b, opposite to c.
-    a = abs(a);
-    b = abs(b);
-    c = abs(c);
-    if (a + b < c || a + c < b || b + c < a) { return NaN; }
-    return acos((a * a + b * b - c * c) / (2 * a * b));
+    a = max(a, -a);
+    b = max(b, -b);
+    c = max(c, -c);
+    if (a + b < c || a + c < b || b + c < a) { return NAN; }
+    return acosf((a * a + b * b - c * c) / (2 * a * b));
 }
 
 float _norm(float a, float b) {
     // Calculate the norm of a vector.
-    return sqrt(a * a + b * b);
+    return sqrtf(a * a + b * b);
 }
 
 bool _inverse_kinematics(float x, float y, float z,
@@ -36,11 +36,10 @@
     // Return true on success, and false if x and y are out of range.
     float f = _norm(x, y) - COXA;
     float d = _norm(f, z);
-    // if (d > FEMUR + TIBIA) { return false; }
-    d = min(d, FEMUR + TIBIA);
-    *hip = atan2(y, x);
+    if (d > FEMUR + TIBIA) { return false; }
+    *hip = atan2f(y, x) - PI4;
     if (isnan(*hip)) { return false; }
-    *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2(-z, f);
+    *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2f(-z, f);
     if (isnan(*knee)) { return false; }
     *ankle = _solve_triangle(FEMUR, TIBIA, d) - PI2;
     if (isnan(*ankle)) { return false; }
@@ -49,19 +48,17 @@
 
 bool move_leg(unsigned char leg, float x, float y, float z) {
     // Move the tip of the leg to x, y. Return false when out of range.
-    float ankle = NaN;
-    float knee = NaN;
-    float hip = NaN;
+    float ankle;
+    float knee;
+    float hip;
+
+    if (!_inverse_kinematics(x, y, z, &ankle, &knee, &hip)) { return false; }
+    servo_move(LEG_JOINT[leg][0], ankle);
+    servo_move(LEG_JOINT[leg][1], knee);
+    servo_move(LEG_JOINT[leg][2], hip);
     leg_position[leg][0] = x;
     leg_position[leg][1] = y;
     leg_position[leg][2] = z;
-    if (!_inverse_kinematics(x, y, z, &ankle, &knee, &hip)) {
-        return false;
-    }
-    hip -= PI4;
-    servo_move(LEG_JOINT[leg][0], ankle);
-    servo_move(LEG_JOINT[leg][1], knee);
-    servo_move(LEG_JOINT[leg][2], hip);
     return true;
 }
 
@@ -75,9 +72,10 @@
 
 bool rotate_leg_by(unsigned char leg, float angle) {
     // Rotate the tip of the leg around the center of robot's body.
+    if (angle == 0.0) { return true; }
     float x = leg_position[leg][0] + BASE;
     float y = leg_position[leg][1] + BASE;
-    float nx = x * cos(angle) - y * sin(angle) - BASE;
-    float ny = x * sin(angle) + y * cos(angle) - BASE;
+    float nx = x * cosf(angle) - y * sinf(angle) - BASE;
+    float ny = x * sinf(angle) + y * cosf(angle) - BASE;
     return move_leg(leg, nx, ny, leg_position[leg][2]);
 }
--- a/start/misc.h	Tue Jun 30 18:05:23 2015 +0200
+++ b/start/misc.h	Wed Jul 01 12:51:53 2015 +0200
@@ -1,11 +1,10 @@
 #ifndef MISC_H
 #define MISC_H
 
-extern const double PI2;
-extern const double PI4;
-extern const double TAU;
-extern const double SQRT2;
-extern const double NaN;
+extern const float PI2;
+extern const float PI4;
+extern const float TAU;
+extern const float SQRT2;
 
 // Use the larger SG90 servos
 #define TOTE2
--- a/start/misc.ino	Tue Jun 30 18:05:23 2015 +0200
+++ b/start/misc.ino	Wed Jul 01 12:51:53 2015 +0200
@@ -1,7 +1,6 @@
 #include "misc.h"
 
-const double PI2 = PI / 2;
-const double PI4 = PI / 4;
-const double TAU = PI * 2;
-const double SQRT2 = 1.4142135623730951;
-const double NaN = 0 / 0;
+const float PI2 = PI / 2;
+const float PI4 = PI / 4;
+const float TAU = PI * 2;
+const float SQRT2 = 1.4142135623730951;