From 3ee01ca05b191135941ffe40920335e7072c0b59 Mon Sep 17 00:00:00 2001 From: Lars H Date: Fri, 17 Nov 2017 13:12:44 +0100 Subject: [PATCH] Initial commit --- LARS.cpp | 432 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ LARS.h | 62 ++++++++ 2 files changed, 494 insertions(+) create mode 100644 LARS.cpp create mode 100644 LARS.h diff --git a/LARS.cpp b/LARS.cpp new file mode 100644 index 0000000..025df2e --- /dev/null +++ b/LARS.cpp @@ -0,0 +1,432 @@ +#include +#include "LARS.h" + +/* + (servo index, pin to attach pwm) + __________ __________ _________________ + |(3,9)_____)(1,8) (0,2)(______(2,3)| + |__| |left FRONT right| |__| + | | + | | + | | + _________ | | __________ + |(7,7)_____)(5,6)______(4,4)(______(6,5)| + |__| |__| + +*/ +//comment below manually setting trim in LARS() constructor +#define __LOAD_TRIM_FROM_EEPROM__ + +#define EEPROM_MAGIC 0xabcd +#define EEPROM_OFFSET 2 //eeprom starting offset to store trim[] + + +LARS::LARS(): reverse{0, 0, 0, 0, 0, 0, 0, 0}, trim{0, 0, 0, 0, 0, 0, 0, 0} { + board_pins[FRONT_RIGHT_HIP] = 26; // front left inner + board_pins[FRONT_LEFT_HIP] = 25; // front right inner + board_pins[BACK_RIGHT_HIP] = 17; // back left inner + board_pins[BACK_LEFT_HIP] = 16; // back right inner + board_pins[FRONT_RIGHT_LEG] = 27; // front left outer + board_pins[FRONT_LEFT_LEG] = 14; // front right outer // 15, 14, 12 free for Lukas + board_pins[BACK_RIGHT_LEG] = 12; // back left outer + board_pins[BACK_LEFT_LEG] = 13; // back right outer +} + +void LARS::init() { + /* + trim[] for calibrating servo deviation, + initial posture (home) should like below + in symmetric + \ / + \_____/ + | | + |_____| + / \ + / \ + */ + /* + trim[FRONT_LEFT_HIP] = 0; + trim[FRONT_RIGHT_HIP] = -8; + trim[BACK_LEFT_HIP] = 8; + trim[BACK_RIGHT_HIP] = 5; + + trim[FRONT_LEFT_LEG] = 2; + trim[FRONT_RIGHT_LEG] = -6; + trim[BACK_LEFT_LEG] = 6; + trim[BACK_RIGHT_LEG] = 5; + */ +#ifdef __LOAD_TRIM_FROM_EEPROM__ + int val = EEPROMReadWord(0); + if (val != EEPROM_MAGIC) { + EEPROMWriteWord(0, EEPROM_MAGIC); + storeTrim(); + } +#endif + + for (int i = 0; i < 8; i++) { + oscillator[i].start(); + servo[i].attach(board_pins[i]); +#ifdef __LOAD_TRIM_FROM_EEPROM__ + int val = EEPROMReadWord(i * 2 + EEPROM_OFFSET); + if (val >= -90 && val <= 90) { + trim[i] = val; + } +#endif + } + + home(); +} + +void LARS::turnR(float steps, float T = 600) { + //int x_amp = 15; + //int z_amp = 15; + int x_amp = 50; + int z_amp = 50; + int ap = 15; + int hi = 23; + //int hi = 0; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {x_amp, x_amp, z_amp, z_amp, x_amp, x_amp, z_amp, z_amp}; + int offset[] = {90 + ap, 90 - ap, 90 - hi, 90 + hi, 90 - ap, 90 + ap, 90 + hi, 90 - hi}; + int phase[] = {0, 180, 90, 90, 180, 0, 90, 90}; + + execute(steps, period, amplitude, offset, phase); +} + +void LARS::turnL(float steps, float T = 600) { + //int x_amp = 15; + //int z_amp = 15; + int x_amp = 50; + int z_amp = 50; + int ap = 15; + int hi = 23; + //int hi = 0; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {x_amp, x_amp, z_amp, z_amp, x_amp, x_amp, z_amp, z_amp}; + int offset[] = {90 + ap, 90 - ap, 90 - hi, 90 + hi, 90 - ap, 90 + ap, 90 + hi, 90 - hi}; + int phase[] = {180, 0, 90, 90, 0, 180, 90, 90}; + + execute(steps, period, amplitude, offset, phase); +} + +void LARS::dance(float steps, float T = 600) { + int x_amp = 0; + int z_amp = 40; + int ap = 30; + int hi = 0; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {x_amp, x_amp, z_amp, z_amp, x_amp, x_amp, z_amp, z_amp}; + int offset[] = {90 + ap, 90 - ap, 90 - hi, 90 + hi, 90 - ap, 90 + ap, 90 + hi, 90 - hi}; + int phase[] = {0, 0, 0, 270, 0, 0, 90, 180}; + + execute(steps, period, amplitude, offset, phase); +} + + +void LARS::omniWalk(float steps, float T, bool side, float turn_factor) { + int x_amp = 15; + int z_amp = 15; + int ap = 15; + int hi = 23; + int front_x = 6 * (1 - pow(turn_factor, 2)); + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {x_amp, x_amp, z_amp, z_amp, x_amp, x_amp, z_amp, z_amp}; + int offset[] = { 90 + ap - front_x, + 90 - ap + front_x, + 90 - hi, + 90 + hi, + 90 - ap - front_x, + 90 + ap + front_x, + 90 + hi, + 90 - hi + }; + + int phase[8]; + if (side) { + int phase1[] = {0, 0, 90, 90, 180, 180, 90, 90}; + int phase2R[] = {0, 180, 90, 90, 180, 0, 90, 90}; + for (int i = 0; i < 8; i++) + phase[i] = phase1[i] * (1 - turn_factor) + phase2R[i] * turn_factor; + } + else { + int phase1[] = {0, 0, 90, 90, 180, 180, 90, 90}; + int phase2L[] = {180, 0, 90, 90, 0, 180, 90, 90}; + for (int i = 0; i < 8; i++) + phase[i] = phase1[i] * (1 - turn_factor) + phase2L[i] * turn_factor + oscillator[i].getPhaseProgress(); + } + + execute(steps, period, amplitude, offset, phase); +} + +void LARS::moonwalkL(float steps, float T = 5000) { + int z_amp = 45; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {0, 0, z_amp, z_amp, 0, 0, z_amp, z_amp}; + int offset[] = {90, 90, 90, 90, 90, 90, 90, 90}; + int phase[] = {0, 0, 0, 120, 0, 0, 180, 290}; + + execute(steps, period, amplitude, offset, phase); +} + + +void LARS::walk(int dir, float steps, float T) { + int x_amp = 15; + int x_amp_test = 50; + int z_amp = 20; + int z_amp_test = 50; + int ap = 20; + int hi = 23; + //int hi = -10; + //int front_x = 12; // inner back, inner back , outer back, outer back, inner front , inner front, outer front , outer front + int front_x = 0; + float period[] ={T , T , T /2, T /2 , T , T , T / 2, T / 2}; //{T, T, T / 2, T / 2, T, T, T / 2, T / 2}; + int amplitude[] = {x_amp, x_amp, z_amp_test, z_amp_test, x_amp, x_amp, z_amp_test, z_amp_test}; + int offset[] = { 90 + ap - front_x, + 90 - ap + front_x, + 90 - hi , + 90 + hi, + 90 - ap - front_x, + 90 + ap + front_x, + 90 + hi, + 90 - hi + /* 90, + 90, + 90, + 90, + 90, + 90, + 90, + 90 */ + }; + //int phase[] = {90, 90, 270, 90, 270, 270, 90, 270}; + int phase[] = {270, 270, 270, 90, 90, 90, 90, 270}; + if (dir == 0) { //backward + phase[0] = phase[1] = 90; + phase[4] = phase[5] = 270; + } + for (int i = 0; i < 8; i++) { + oscillator[i].reset(); + oscillator[i].setPeriod(period[i]); + oscillator[i].setAmplitude(amplitude[i]); + oscillator[i].setPhase(phase[i]); + oscillator[i].setOffset(offset[i]); + } + + _final_time = millis() + period[0] * steps; + _init_time = millis(); + bool side; + while (millis() < _final_time) { + side = (int)((millis() - _init_time) / (period[0] / 2)) % 2; + setServo(0, oscillator[0].refresh()); + setServo(1, oscillator[1].refresh()); + setServo(4, oscillator[4].refresh()); + setServo(5, oscillator[5].refresh()); + + if (side == 0) { + setServo(3, oscillator[3].refresh()); + setServo(6, oscillator[6].refresh()); + } + else { + setServo(2, oscillator[2].refresh()); + setServo(7, oscillator[7].refresh()); + } + delay(1); + } +} + +void LARS::upDown(float steps, float T = 5000) { + int x_amp = 0; + int z_amp = 35; + int ap = 20; + //int hi = 25; + int hi = 0; + int front_x = 0; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {x_amp, x_amp, z_amp, z_amp, x_amp, x_amp, z_amp, z_amp}; + int offset[] = { 90 + ap - front_x, + 90 - ap + front_x, + 90 - hi, + 90 + hi, + 90 - ap - front_x, + 90 + ap + front_x, + 90 + hi, + 90 - hi + }; + int phase[] = {0, 0, 90, 270, 180, 180, 270, 90}; + + execute(steps, period, amplitude, offset, phase); +} + + +void LARS::pushUp(float steps, float T = 600) { + int z_amp = 40; + int x_amp = 65; + int hi = 0; + int Position =60; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {0, 0, z_amp, z_amp, 0, 0, 0, 0}; + int offset[] = {90, 90, 90 - hi, 90 + hi, 90 - x_amp, 90 + x_amp, 90 + hi - Position, 90 - hi + Position}; + int phase[] = {0, 0, 0, 180, 0, 0, 0, 180}; + + execute(steps, period, amplitude, offset, phase); +} + +void LARS::hello() { + float sentado[] = {90 + 15, 90 - 15, 90 - 65, 90 + 65, 90 + 20, 90 - 20, 90 + 10, 90 - 10}; + moveServos(150, sentado); + delay(200); + + int z_amp = 40; + int x_amp = 60; + int T = 350; + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {0, 50, 0, 50, 0, 0, 0, 0}; + int offset[] = { + 90 + 15, 40, + 90 - 10, 90 + 10, + 90 + 20, 90 - 20, + 90 + 65, 90 + }; + + int phase[] = {0, 0, 0, 90, 0, 0, 0, 0}; + + execute(4, period, amplitude, offset, phase); + + float goingUp[] = {160, 20, 90, 90, 90 - 20, 90 + 20, 90 + 10, 90 - 10}; + moveServos(500, goingUp); + delay(200); + +} + +void LARS::wave(){ + + int z_amp = 40; + int x_amp = 60; + int T = 350; + + float period[] = {T, T, T, T, T, T, T, T}; + int amplitude[] = {0, 60, 0, 0, 0, 0, 0, 0}; + int offset[] = { + 90-20 , 90-60, + 90 , 90+70 , + 90 , 90+90 , + 90+140 , 90 + }; + + int phase[] = {0, 0, 0, 90, 0, 0, 0, 0}; + execute(3, period, amplitude, offset, phase); + delay (200); +} + + +void LARS::home() { + int ap = 20; + //int hi = 35; + int hi = 0; + int position[] = {90 + ap, 90 - ap, 90 - hi, 90 + hi, 90 - ap, 90 + ap, 90 + hi, 90 - hi}; + for (int i = 0; i < 8; i++) { + if (position[i] + trim[i] <= 180 && position[i] + trim[i] > 0) { + setServo(i, position[i] + trim[i]); + } + } +} + + + +void LARS::reverseServo(int id) { + if (reverse[id]) + reverse[id] = 0; + else + reverse[id] = 1; +} + + +void LARS::setServo(int id, float target) { + if (!reverse[id]) + servo[id].writeMicroseconds(angToUsec(target + trim[id])); + else + servo[id].writeMicroseconds(angToUsec(180 - (target + trim[id]))); + _servo_position[id] = target + trim[id]; +} + +float LARS::getServo(int id) { + return _servo_position[id]; +} + + +void LARS::moveServos(int time, float target[8]) { + if (time > 10) { + for (int i = 0; i < 8; i++) _increment[i] = (target[i] - _servo_position[i]) / (time / 10.0); + _final_time = millis() + time; + + while (millis() < _final_time) { + _partial_time = millis() + 10; + for (int i = 0; i < 8; i++) setServo(i, _servo_position[i] + _increment[i]); + while (millis() < _partial_time); //pause + } + } + else { + for (int i = 0; i < 8; i++) setServo(i, target[i]); + } + for (int i = 0; i < 8; i++) _servo_position[i] = target[i]; +} + +void LARS::execute(float steps, float period[8], int amplitude[8], int offset[8], int phase[8]) { + + for (int i = 0; i < 8; i++) { + oscillator[i].setPeriod(period[i]); + oscillator[i].setAmplitude(amplitude[i]); + oscillator[i].setPhase(phase[i]); + oscillator[i].setOffset(offset[i]); + } + + unsigned long global_time = millis(); + + for (int i = 0; i < 8; i++) oscillator[i].setTime(global_time); + + _final_time = millis() + period[0] * steps; + while (millis() < _final_time) { + for (int i = 0; i < 8; i++) { + setServo(i, oscillator[i].refresh()); + } + yield(); + } +} +void LARS::storeTrim() { + for (int i = 0; i < 8; i++) { + EEPROMWriteWord(i * 2 + EEPROM_OFFSET, trim[i]); + delay(100); + } + +} + +// load/send only trim of hip servo +void LARS::loadTrim() { + //FRONT_LEFT/RIGHT_HIP + for (int i = 0; i < 4; i++) { + Serial.write(EEPROM.read(i + EEPROM_OFFSET)); + } + + //BACK_LEFT/RIGHT_HIP + for (int i = 8; i < 12; i++) { + Serial.write(EEPROM.read(i + EEPROM_OFFSET)); + } +} + +int LARS::EEPROMReadWord(int p_address) +{ + byte lowByte = EEPROM.read(p_address); + byte highByte = EEPROM.read(p_address + 1); + + return ((lowByte << 0) & 0xFF) + ((highByte << 8) & 0xFF00); +} + +void LARS::EEPROMWriteWord(int p_address, int p_value) +{ + byte lowByte = ((p_value >> 0) & 0xFF); + byte highByte = ((p_value >> 8) & 0xFF); + + EEPROM.write(p_address, lowByte); + EEPROM.write(p_address + 1, highByte); +} + diff --git a/LARS.h b/LARS.h new file mode 100644 index 0000000..9dedd1a --- /dev/null +++ b/LARS.h @@ -0,0 +1,62 @@ +#ifndef LARS_h +#define LARS_h +#include "Octosnake.h" + +// servo index to board_pins + +#define FRONT_RIGHT_HIP 0 +#define FRONT_LEFT_HIP 1 +#define FRONT_RIGHT_LEG 2 +#define FRONT_LEFT_LEG 3 +#define BACK_RIGHT_HIP 4 +#define BACK_LEFT_HIP 5 +#define BACK_RIGHT_LEG 6 +#define BACK_LEFT_LEG 7 + +class LARS { + + public: + LARS(); + void init(); + void walk(int dir = 1, float steps = 1, float T = 400); + void omniWalk(float steps, float T, bool side, float turn_factor); + void turnL(float steps, float period); + void turnR(float steps, float period); + void moonwalkL(float steps, float period); + void dance(float steps, float period); + void upDown(float steps, float period); + void pushUp(float steps, float period); + void hello(); + void home(); + void wave(); + + void setServo(int id, float target); + void reverseServo(int id); + float getServo(int id); + void moveServos(int time, float target[8]); + void setTrim(int index, int value) { + trim[index] = value; + } + void storeTrim(); + void loadTrim(); + private: + Oscillator oscillator[8]; + Servo servo[8]; + int board_pins[8]; + int trim[8]; //deviation servo offset + bool reverse[8]; + unsigned long _init_time; + unsigned long _final_time; + unsigned long _partial_time; + float _increment[8]; + float _servo_position[8]; + + void execute(float steps, float period[8], int amplitude[8], int offset[8], int phase[8]); + inline int angToUsec(float value) { + return value / 180 * (MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) + MIN_PULSE_WIDTH; + }; + int EEPROMReadWord(int p_address); + void EEPROMWriteWord(int p_address, int p_value); +}; + +#endif