Mercurial > tote

changeset 159:806a193584f9 teensy31

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Use int instead of signed char
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Thu, 02 Jul 2015 14:58:46 +0200
parents 4286f9773694
children 162ec23c4fbe
files start/creep.ino start/leg.h start/leg.ino start/servos.ino start/start.ino
diffstat 5 files changed, 116 insertions(+), 10 deletions(-) [+]
line wrap: on
line diff
--- a/start/creep.ino	Wed Jul 01 12:51:53 2015 +0200
+++ b/start/creep.ino	Thu Jul 02 14:58:46 2015 +0200
@@ -11,11 +11,11 @@
 /* How much to shift the body? */
 const float SHIFT = 8.0;
 /* In how many steps should the body be shifted? */
-const unsigned char SHIFT_STEPS = 2;
+const int SHIFT_STEPS = 2;
 /* How fast to raise the leg. */
 const float RAISE = 10.0;
 /* In how many steps. */
-const unsigned char RAISE_STEPS = 3;
+const int RAISE_STEPS = 3;
 
 /* Given current leg, which leg to move next? */
 const unsigned char _NEXT_LEG[6][4] = {
@@ -68,7 +68,19 @@
 
 /* Shifts the whole body by defined amount. */
 void _shift_body_by(float dx, float dy) {
+    Serial.println("_shift_body_by()");
+    Serial.print("dx = ");
+    Serial.println(dx);
+    Serial.print("dy = ");
+    Serial.println(dy);
+
     for (unsigned char leg = 0; leg < 4; ++leg) {
+        Serial.print("leg = ");
+        Serial.println(leg);
+        Serial.print("LEG_X[leg] = ");
+        Serial.println(LEG_X[leg], DEC);
+        Serial.print("LEG_Y[leg] = ");
+        Serial.println(LEG_Y[leg], DEC);
         move_leg_by(leg, dx * LEG_X[leg], dy * LEG_Y[leg], 0);
     }
     _shift_x += dx;
@@ -77,8 +89,31 @@
 
 /* Shifts the body away from the given leg, for balance. */
 void _shift_body(unsigned char leg) {
-    float dx = (LEG_X[leg] * SHIFT - _shift_x) / SHIFT_STEPS;
-    float dy = (LEG_Y[leg] * SHIFT - _shift_y) / SHIFT_STEPS;
+    Serial.println("_shift_body()");
+    Serial.print("leg = ");
+    Serial.println(leg);
+    Serial.print("LEG_X[leg] = ");
+    Serial.println(LEG_X[leg]);
+    Serial.print("SHIFT = ");
+    Serial.println(SHIFT);
+    float dx = LEG_X[leg] * SHIFT;
+    Serial.print("dx = ");
+    Serial.println(dx);
+    dx -= _shift_x;
+    Serial.print("dx = ");
+    Serial.println(dx);
+    dx /= SHIFT_STEPS;
+    Serial.print("dx = ");
+    Serial.println(dx);
+    float dy = LEG_Y[leg] * SHIFT;
+    Serial.print("dy = ");
+    Serial.println(dy);
+    dy -= _shift_y;
+    Serial.print("dy = ");
+    Serial.println(dy);
+    dy /= SHIFT_STEPS;
+    Serial.print("dy = ");
+    Serial.println(dy);
     for (unsigned char i = 0; i < SHIFT_STEPS; ++i) {
         _shift_body_by(dx, dy);
         _creep_tick();
--- a/start/leg.h	Wed Jul 01 12:51:53 2015 +0200
+++ b/start/leg.h	Thu Jul 02 14:58:46 2015 +0200
@@ -6,13 +6,14 @@
 const float TIBIA = 38.5;
 const float BASE = 21.0;
 
-const char LEG_X[4] = {-1, -1, 1, 1};
-const char LEG_Y[4] = {1, -1, -1, 1};
-extern float leg_position[4][3];
+const int LEG_X[4] = {-1, -1, 1, 1};
+const int LEG_Y[4] = {1, -1, -1, 1};
 const float HOME = (COXA + FEMUR) / SQRT2;
 
 bool move_leg(unsigned char leg, float x, float y, float z);
 bool move_leg_by(unsigned char leg, float dx, float dy, float dz);
 bool rotate_leg_by(unsigned char leg, float angle);
 
+extern float leg_position[4][3];
+
 #endif
--- a/start/leg.ino	Wed Jul 01 12:51:53 2015 +0200
+++ b/start/leg.ino	Thu Jul 02 14:58:46 2015 +0200
@@ -27,15 +27,38 @@
 
 float _norm(float a, float b) {
     // Calculate the norm of a vector.
-    return sqrtf(a * a + b * b);
+    Serial.println("_norm()");
+    Serial.print("a = ");
+    Serial.println(a);
+    Serial.print("b = ");
+    Serial.println(b);
+    float result = sqrtf(a * a + b * b);
+    Serial.print("result = ");
+    Serial.println(result);
+    return result;
 }
 
 bool _inverse_kinematics(float x, float y, float z,
                          float *ankle, float *knee, float *hip) {
     // Calculate angles for knee and ankle, and put them in those variables.
     // Return true on success, and false if x and y are out of range.
+
+    Serial.println("_inverse_kinematics()");
+    Serial.print("COXA = ");
+    Serial.println(COXA);
+    Serial.print("FEMUR = ");
+    Serial.println(FEMUR);
+    Serial.print("TIBIA = ");
+    Serial.println(TIBIA);
+
     float f = _norm(x, y) - COXA;
     float d = _norm(f, z);
+
+    Serial.print("f = ");
+    Serial.println(f);
+    Serial.print("d = ");
+    Serial.println(d);
+
     if (d > FEMUR + TIBIA) { return false; }
     *hip = atan2f(y, x) - PI4;
     if (isnan(*hip)) { return false; }
@@ -52,7 +75,25 @@
     float knee;
     float hip;
 
+    Serial.println("move_leg()");
+    Serial.print("leg = ");
+    Serial.println(leg);
+    Serial.print("x = ");
+    Serial.println(x);
+    Serial.print("y = ");
+    Serial.println(y);
+    Serial.print("z = ");
+    Serial.println(z);
+
     if (!_inverse_kinematics(x, y, z, &ankle, &knee, &hip)) { return false; }
+
+    Serial.print("ankle = ");
+    Serial.println(ankle);
+    Serial.print("knee = ");
+    Serial.println(knee);
+    Serial.print("hip = ");
+    Serial.println(hip);
+
     servo_move(LEG_JOINT[leg][0], ankle);
     servo_move(LEG_JOINT[leg][1], knee);
     servo_move(LEG_JOINT[leg][2], hip);
@@ -64,6 +105,19 @@
 
 bool move_leg_by(unsigned char leg, float dx, float dy, float dz) {
     // Move the tip of the leg by dx, dy. Return false when out of range.
+    Serial.println("move_leg_by()");
+    Serial.print("dx = ");
+    Serial.println(dx);
+    Serial.print("dy = ");
+    Serial.println(dy);
+    Serial.print("dz = ");
+    Serial.println(dz);
+    Serial.print("leg_position[leg][0] = ");
+    Serial.println(leg_position[leg][0]);
+    Serial.print("leg_position[leg][1] = ");
+    Serial.println(leg_position[leg][1]);
+    Serial.print("leg_position[leg][2] = ");
+    Serial.println(leg_position[leg][2]);
     return move_leg(leg,
                     leg_position[leg][0] + dx,
                     leg_position[leg][1] + dy,
@@ -72,10 +126,25 @@
 
 bool rotate_leg_by(unsigned char leg, float angle) {
     // Rotate the tip of the leg around the center of robot's body.
+    Serial.println("rotate_leg_by()");
+    Serial.print("leg = ");
+    Serial.println(leg);
+    Serial.print("angle = ");
+    Serial.println(angle);
+    Serial.print("BASE = ");
+    Serial.println(BASE);
     if (angle == 0.0) { return true; }
     float x = leg_position[leg][0] + BASE;
     float y = leg_position[leg][1] + BASE;
+    Serial.print("x = ");
+    Serial.println(x);
+    Serial.print("y = ");
+    Serial.println(y);
     float nx = x * cosf(angle) - y * sinf(angle) - BASE;
     float ny = x * sinf(angle) + y * cosf(angle) - BASE;
+    Serial.print("nx = ");
+    Serial.println(nx);
+    Serial.print("ny = ");
+    Serial.println(ny);
     return move_leg(leg, nx, ny, leg_position[leg][2]);
 }
--- a/start/servos.ino	Wed Jul 01 12:51:53 2015 +0200
+++ b/start/servos.ino	Thu Jul 02 14:58:46 2015 +0200
@@ -18,14 +18,14 @@
     9, 8, 7,        // hind right
     10, 11, 12      // front right
 };
-static const char SERVO_REVERSE[SERVOS] = {
+static const int SERVO_REVERSE[SERVOS] = {
 // ankle, knee, hip
     -1, 1, 1,    // front left
     1, -1, -1,   // hind left
     -1, 1, 1,    // hind right
     1, -1, -1    // front right
 };
-static const char SERVO_TRIM[SERVOS] = {
+static const int SERVO_TRIM[SERVOS] = {
 // ankle, knee, hip
     0, 0, 0,    // front left
     0, 0, 0,    // hind left
--- a/start/start.ino	Wed Jul 01 12:51:53 2015 +0200
+++ b/start/start.ino	Thu Jul 02 14:58:46 2015 +0200
@@ -11,6 +11,7 @@
 
 
 void setup() {
+    Serial.begin(115200);
     servo_setup();
     pinMode(13, OUTPUT);
     digitalWrite(13, HIGH);