Mercurial > pkubik
changeset 17:07e5364d368e default tip
Add a high-level arduino controller
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Thu, 26 Feb 2015 18:18:06 +0100 |
| parents | dd935af37b17 |
| children | |
| files | controller/battery.ino controller/beep.ino controller/servos.ino dice/battery.h dice/battery.ino dice/beep.h dice/beep.ino dice/clock.h dice/clock.ino dice/creep.h dice/creep.ino dice/dice.ino dice/leg.h dice/leg.ino dice/misc.h dice/misc.ino dice/serial.h dice/serial.ino dice/servos.h dice/servos.ino |
| diffstat | 20 files changed, 590 insertions(+), 24 deletions(-) [+] |
line wrap: on
line diff
--- a/controller/battery.ino Sat Feb 14 14:47:37 2015 +0100 +++ b/controller/battery.ino Thu Feb 26 18:18:06 2015 +0100 @@ -1,4 +1,4 @@ -#define BATTERY_PIN A5 +#define BATTERY_PIN A4 void battery_setup() { pinMode(BATTERY_PIN, INPUT); @@ -12,9 +12,8 @@ static int battery_fail = 0; int reading = get_battery(); - if (reading < 100) { + if (reading < 40) { battery_fail += 1; - tone(12, reading, 200); } else { battery_fail = 0; }
--- a/controller/beep.ino Sat Feb 14 14:47:37 2015 +0100 +++ b/controller/beep.ino Thu Feb 26 18:18:06 2015 +0100 @@ -1,4 +1,4 @@ -#define BEEP_PIN 12 +#define BEEP_PIN 3 void beep_setup() { pinMode(BEEP_PIN, OUTPUT);
--- a/controller/servos.ino Sat Feb 14 14:47:37 2015 +0100 +++ b/controller/servos.ino Thu Feb 26 18:18:06 2015 +0100 @@ -8,41 +8,41 @@ static const unsigned char SERVO_PINS[SERVOS] = { // ankle, knee, hip - 2, 3, 5, // front left - 9, 8, 6, // hind left - 10, 11, 13, // hind right - 17, 16, 14 // front right + 4, 5, 6, // front left + 9, 8, 7, // hind left + 10, 11, 12, // hind right + 16, 15, 14 // front right }; static const 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 + -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] = { // ankle, knee, hip - -6, 0, -8, // front left - 2, -8, -8, // hind left - -4, 0, 4, // hind right - 0, 2, 0 // front right + -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 - -PI4, 0, 0, // front left - -PI4, 0, 0, // hind left - -PI4, 0, 0, // hind right - -PI4, 0, 0 // front right + 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 - 0, -PI2, -PI4, // front left - 0, -PI2, PI4, // hind left - 0, -PI2, PI4, // hind right - 0, -PI2, -PI4 // front right + 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) {
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/battery.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,8 @@ +#ifndef BATTERY_H +#define BATTERY_H + +void battery_setup(void); +int get_battery(void); +void battery_check(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/battery.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,29 @@ +#include "beep.h" + +#define BATTERY_PIN A3 +#define MIN_BATTERY 368 // About 3.59V. + + +void battery_setup() { + pinMode(BATTERY_PIN, INPUT); +} + +int get_battery() { + return analogRead(BATTERY_PIN); +} + +void battery_check() { + static int battery_fail = 0; + int reading = get_battery(); + + if (reading < MIN_BATTERY) { + battery_fail += 1; + beep(reading, 200); + } else { + battery_fail = 0; + } + if (battery_fail > 40) { + /* If it stays bad for 4s, shut down. */ + servo_shutdown(); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/beep.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,9 @@ +#ifndef BEEP_H +#define BEEP_H + +extern bool beep_mute; + +void beep_setup(void); +void beep(int frequency, int duration); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/beep.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,20 @@ +#define BEEP_PIN 3 + +bool beep_mute = true; + + +void beep_setup() { + pinMode(BEEP_PIN, OUTPUT); + beep(1760, 50); + delay(100); + beep(1975, 50); + delay(50); + beep(1760, 50); +} + +void beep(int frequency, int duration) { + if (beep_mute) { + return; + } + tone(BEEP_PIN, frequency, duration); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/clock.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,9 @@ +#ifndef CLOCK_H +#define CLOCK_H + +#define FPS 32 + +const unsigned char TICK = 1000 / FPS; +void tick(unsigned int ms); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/clock.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,14 @@ +#include "clock.h" + +void tick(unsigned int ms) { + // Wait remaining time to delay ms milliseconds since last call. + static unsigned long last = 0; + unsigned long now = millis(); + long wait = last + ms - now; + if (wait > 0) { + delay(wait); + } else { + // Somehow engage a time machine here to go back in time. + } + last = now; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/creep.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,14 @@ +#ifndef CREEP_H +#define CREEP_H + +/* Walking speed. */ +extern double creep_dx; // Sideways. +extern double creep_dy; // Forward. +extern double creep_spread; +extern double creep_rotation; +extern double creep_height; + +/* Perform one step of the creep gait with the current speed. */ +void creep(); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/creep.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,126 @@ +#include "creep.h" +#include "misc.h" +#include "leg.h" +#include "clock.h" +#include "beep.h" + + +const unsigned int tones[4] = {880, 988, 1318, 1175}; + +/* Creep gait parameters. */ + +/* How far forward to move them when making a step? */ +#define STRIDE 12.0 + +/* How much to shift the body? */ +#define SHIFT 8.0 + +/* In how many steps should the body be shifted? */ +#define STEPS 4 + +/* What is the home position of the legs? */ +#define HOME ((COXA + FEMUR) / SQRT2) + +/* Given current leg, which leg to move next? */ +const unsigned char _NEXT_LEG[4] = {2, 0, 3, 1}; + +/* Body position. */ +static double _shift_x = 0.0; +static double _shift_y = 0.0; + +/* Which legs are touching the ground? */ +static bool _on_ground[4] = {true, true, true, true}; + +/* Walking speed. */ +double creep_dx = 0.0; // Sideways. +double creep_dy = 0.0; // Forward. +double creep_rotation = 0.0; // Rotation. +double creep_height = TIBIA * 0.75; // Body height. +double creep_spread = 0.0; // Leg spread. + +/* +Move all legs that are on the ground backwards, pushing the robot forward with +the current walking speed. This is called every tick, to make the robot move +continuously. +*/ +void _creep_move() { + for (unsigned char leg=0; leg < 4; ++leg) { + if (_on_ground[leg]) { + move_leg( + leg, + leg_position[leg][0] - creep_dx * LEG_X[leg] / 2, + leg_position[leg][1] - creep_dy * LEG_Y[leg] / 2, + -creep_height + ); + rotate_leg_by(leg, -creep_rotation * LEG_X[leg] * LEG_Y[leg] / 2); + } + } +} + +/* Called every frame. */ +void _creep_tick() { + _creep_move(); + tick(TICK); +} + +/* Shifts the whole body by defined amount. */ +void _shift_body_by(double dx, double dy) { + for (unsigned char leg=0; leg < 4; ++leg) { + move_leg_by(leg, dx * LEG_X[leg], dy * LEG_Y[leg], 0); + } + _shift_x += dx; + _shift_y += dy; +} + +/* Shifts the body away from the given leg, for balance. */ +void _shift_body(unsigned char leg) { + double dx = (LEG_X[leg] * SHIFT - _shift_x) / STEPS; + double dy = (LEG_Y[leg] * SHIFT - _shift_y) / STEPS; + for (unsigned char i = 0; i < STEPS; ++i) { + _shift_body_by(dx, dy); + _creep_tick(); + } +} + +/* Perform a full step with a single leg. */ +void _creep_step(unsigned char leg) { + _shift_body(leg); + _on_ground[leg] = false; + beep(440, 5); + for (unsigned char i = 0; i < 3; ++i) { + move_leg_by(leg, 0, 0, 10); + _creep_tick(); + } + beep(tones[leg], 25); + move_leg( + leg, + HOME + (_shift_x + creep_dx * STRIDE) * LEG_X[leg] + creep_spread, + HOME + (_shift_y + creep_dy * STRIDE) * LEG_Y[leg] + creep_spread, + leg_position[leg][2] + ); + _creep_tick(); + _creep_tick(); + _creep_tick(); + _on_ground[leg] = true; + beep(1865, 5); + for (unsigned char i = 0; i < 3; ++i) { + move_leg_by(leg, 0, 0, -10); + _creep_tick(); + } +} + +/* Perform one step of the creep gait with the current speed. */ +void creep() { + static unsigned char leg = 0; + + if ( + (creep_dx < -0.01 || creep_dx > 0.01) || + (creep_dy < -0.01 || creep_dy > 0.01) || + (creep_rotation < -0.01 || creep_rotation > 0.01) + ) { + _creep_step(leg); + leg = _NEXT_LEG[leg]; + } else { + _creep_tick(); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/dice.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,21 @@ +#include "servos.h" +#include "beep.h" +#include "battery.h" +#include "creep.h" +#include "leg.h" +#include "misc.h" +#include "serial.h" + + +void setup() { + battery_setup(); + servo_setup(); + beep_setup(); + serial_setup(); +} + + +void loop() { + creep(); + serial_loop(); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/leg.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,17 @@ +#ifndef LEG_H +#define LEG_H + +#define COXA 8.0 +#define FEMUR 20.5 +#define TIBIA 28.5 +#define BASE 18.5 + +const char LEG_X[4] = {-1, -1, 1, 1}; +const char LEG_Y[4] = {1, -1, -1, 1}; +extern double leg_position[4][3]; + +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/dice/leg.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,82 @@ +#include "leg.h" +#include "misc.h" + +#define HOME ((COXA + FEMUR) / SQRT2) +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); + if (a + b < c || a + c < b || b + c < a) { return NaN; } + 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 = min(FEMUR + TIBIA, _norm(f, z)); + *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; + 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); + leg_position[leg][0] = x; + leg_position[leg][1] = y; + leg_position[leg][2] = z; + 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/dice/misc.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +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; + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/misc.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,7 @@ +#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;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/serial.h Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,7 @@ +#ifndef SERIAL_H +#define SERIAL_H + +void serial_setup(void); +void serial_loop(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/serial.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,81 @@ +#include "creep.h" +#include "leg.h" +#include "servos.h" +#include "misc.h" + +void serial_setup() { + int16_t prev = 0x00; + int16_t next = 0x00; + + Serial.begin(19200); + + while (prev != 'x' || next != 'x') { + if (Serial.available()) { + prev = next; + next = Serial.read(); + } + delay(100); + } +} + +void serial_loop() { + while (Serial.available()) { + unsigned char command = Serial.read(); + switch (command) { + case 'w': // Up arrow. + creep_dy = min(1.5, creep_dy + 0.25); + break; + case 's': // Down arrow. + creep_dy = max(-1.5, creep_dy - 0.25); + break; + case 'a': // Left arrow. + creep_rotation = min(PI/90, creep_rotation + PI/360); + break; + case 'd': // Right arrow. + creep_rotation = max(-PI/90, creep_rotation - PI/360); + break; + case '[': // Rewind. + creep_spread = max(-10.0, creep_spread - 1); + break; + case '\'': // Play/pause. + creep_spread = 0.0; + break; + case ']': // Fast forward. + creep_spread = min(10.0, creep_spread + 1); + break; + case ',': // Channel minus. + creep_height = max(TIBIA * 0.25, creep_height - 1); + break; + case '.': // Channel plus. + creep_height = min(TIBIA * 1.25, creep_height + 1); + break; + case '/': + creep_height = TIBIA * 0.75; + break; + case 'z': // Volume down. + creep_dx = max(-1.0, creep_dx - 0.25); + break; + case 'm': // Mute. + beep_mute = !beep_mute; + break; + case 'c': // Volume up. + creep_dx = min(1.0, creep_dx + 0.25); + break; + case 'h': // Home. + for (unsigned char leg=0; leg < 4; ++leg) { + move_leg( + leg, + ((COXA + FEMUR) / SQRT2), + ((COXA + FEMUR) / SQRT2), + TIBIA + ); + } + break; + case 'x': // OK. + creep_dx = 0.0; + creep_dy = 0.0; + creep_rotation = 0.0; + break; + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/dice/servos.h Thu Feb 26 18:18:06 2015 +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/dice/servos.ino Thu Feb 26 18:18:06 2015 +0100 @@ -0,0 +1,102 @@ +#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 + 6, 5, 4, // front left + 7, 8, 9, // hind left + 12, 11, 10, // hind right + 14, 15, 16 // front right +}; +static const 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 char SERVO_TRIM[SERVOS] = { +// ankle, knee, hip + 0, 0, 10, // front left + 0, 0, 10, // hind left + -10, -5, -10, // hind right + 10, -5, -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(); + } +}