Mercurial > tote-had
changeset 1:1af8f7738de1
Initial controller code
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sun, 06 Mar 2016 21:10:25 +0100 |
| parents | 77727770d6f8 |
| children | c694cfc44d91 |
| files | controller/controller.ino controller/i2c.ino controller/leg.h controller/leg.ino controller/misc.h controller/misc.ino controller/servos.h controller/servos.ino |
| diffstat | 8 files changed, 278 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/controller.ino Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,14 @@ +#include "misc.h" +#include "servos.h" +#include "leg.h" +#include "i2c.h" + +void setup() { + servo_setup(); + i2c_setuo(); +} + +void loop() { + delay(100); + i2c_loop() +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/i2c.ino Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,16 @@ +#include <Wire.h> + + +const int SLAVE_ADDRESS = 0x09; + + +void i2c_setup() { + Wire.begin(SLAVE_ADDRESS); + Wire.onReceive(receiveEvent); +} + +void receiveEvent(int bytes) { +} + +void i2c_loop() { +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/leg.h Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,27 @@ +#ifndef LEG_H +#define LEG_H + +#ifdef TOTE2 +// For version 2: +#define COXA 25.0 +#define FEMUR 42.0 +#define TIBIA 38.5 +#define BASE 21.0 +#else +// For version 1: +#define COXA 8.0 +#define FEMUR 20.5 +#define TIBIA 28.5 +#define BASE 18.5 +#endif + +const signed char LEG_X[4] = {-1, -1, 1, 1}; +const signed char LEG_Y[4] = {1, -1, -1, 1}; +extern double leg_position[4][3]; +const double HOME = (COXA + FEMUR) / SQRT2; + +bool move_leg(unsigned char leg, double x, double y, double z); +bool move_leg_by(unsigned char leg, double dx, double dy, double dz); +bool rotate_leg_by(unsigned char leg, double angle); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/leg.ino Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,84 @@ +#include "misc.h" +#include "leg.h" + +const unsigned char LEG_JOINT[4][3] = { + {0, 1, 2}, + {3, 4, 5}, + {6, 7, 8}, + {9, 10, 11}, +}; + +double leg_position[4][3] = { + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, +}; + + +double _solve_triangle(double a, double b, double c) { + // Calculate the angle between a and b, opposite to c. + a = abs(a); + b = abs(b); + c = abs(c); + c = min(c, a + b); + b = min(b, a + c); + a = min(a, b + c); + return acos((a * a + b * b - c * c) / (2 * a * b)); +} + +double _norm(double a, double b) { + // Calculate the norm of a vector. + return sqrt(a * a + b * b); +} + +bool _inverse_kinematics(double x, double y, double z, + double *ankle, double *knee, double *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. + double f = _norm(x, y) - COXA; + double d = _norm(f, z); + d = min(d, FEMUR + TIBIA); + *hip = atan2(y, x); + if (isnan(*hip)) { return false; } + *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2(-z, f); + if (isnan(*knee)) { return false; } + *ankle = _solve_triangle(FEMUR, TIBIA, d) - PI2; + if (isnan(*ankle)) { return false; } + return true; +} + +bool move_leg(unsigned char leg, double x, double y, double z) { + // Move the tip of the leg to x, y. Return false when out of range. + double ankle = NAN; + double knee = NAN; + double hip = NAN; + 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; +} + +bool move_leg_by(unsigned char leg, double dx, double dy, double dz) { + // Move the tip of the leg by dx, dy. Return false when out of range. + return move_leg(leg, + leg_position[leg][0] + dx, + leg_position[leg][1] + dy, + leg_position[leg][2] + dz); +} + +bool rotate_leg_by(unsigned char leg, double angle) { + // Rotate the tip of the leg around the center of robot's body. + double x = leg_position[leg][0] + BASE; + double y = leg_position[leg][1] + BASE; + double nx = x * cos(angle) - y * sin(angle) - BASE; + double ny = x * sin(angle) + y * cos(angle) - BASE; + return move_leg(leg, nx, ny, leg_position[leg][2]); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/misc.h Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,12 @@ +#ifndef MISC_H +#define MISC_H + +extern const double PI2; +extern const double PI4; +extern const double TAU; +extern const double SQRT2; + +// Use the larger SG90 servos +#define TOTE2 + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/misc.ino Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +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;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/servos.h Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,11 @@ +#ifndef SERVOS_H +#define SERVOS_H + +// Number of servos. +#define SERVOS 12 + +void servo_move(unsigned char servo, double rads); +void servo_setup(void); +void servo_shutdown(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller/servos.ino Sun Mar 06 21:10:25 2016 +0100 @@ -0,0 +1,108 @@ +#include "misc.h" +#include "servos.h" +#include <Servo.h> + +// Range of the servos. +#define SERVO_FREQ_MIN 600 +#define SERVO_FREQ_MAX 2400 + +static const unsigned char SERVO_PINS[SERVOS] = { +// ankle, knee, hip + +/* For Pro Micro + 14, 16, 10, // front left + A0, A1, A2, // hind left +*/ + + 12, 11, 10, // front left + 14, 15, 16, // hind left + 6, 5, 4, // hind right + 7, 8, 9 // front right +}; +static const signed char 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 signed char SERVO_TRIM[SERVOS] = { +// ankle, knee, hip + -10, -5, -10, // front left + 10, -5, -10, // hind left + 0, 0, 10, // hind right + 0, 0, 10 // front right +}; +static Servo servos[SERVOS]; + +static const double HOME_POSITION[SERVOS] = { +// ankle, knee, hip + 0, 0, 0, // front left + 0, 0, 0, // hind left + 0, 0, 0, // hind right + 0, 0, 0 // front right +}; + +static const double FOLD_POSITION[SERVOS] = { +// ankle, knee, hip + -PI2, PI2, -PI4, // front left + -PI2, PI2, -PI4, // hind left + -PI2, PI2, -PI4, // hind right + -PI2, PI2, -PI4 // front right +}; + +void servo_move(unsigned char servo, double rads) { + // Move a servo to a position in radians between -PI/2 and PI/2. + if (servo >= SERVOS) { + return; + } + rads = rads * SERVO_REVERSE[servo] + PI2; + while (rads > TAU) { + rads -= TAU; + } + while (rads < 0) { + rads += TAU; + } + int freq = (SERVO_FREQ_MAX - + SERVO_FREQ_MIN) * rads / PI + SERVO_FREQ_MIN; + freq = min(SERVO_FREQ_MAX, + max(SERVO_FREQ_MIN, + freq + SERVO_TRIM[servo] * 10)); + servos[servo].writeMicroseconds(freq); +} + +void servo_setup() { + // Initialize all servos. + // Start with hips. + for (unsigned char servo = 2; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } + // Then ankles. + for (unsigned char servo = 0; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } + // Finally knees. + for (unsigned char servo = 1; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } +} + +void servo_shutdown() { + // Power down all servos. + for (unsigned char servo = 0; servo < SERVOS; ++servo) { + servo_move(servo, FOLD_POSITION[servo]); + } + delay(500); + for (unsigned char servo = 0; servo < SERVOS; ++servo) { + servos[servo].detach(); + } +}