Initial commit
This commit is contained in:
Lars H
commit
3ee01ca05b
432
LARS.cpp
Normal file
432
LARS.cpp
Normal file
@ -0,0 +1,432 @@
|
||||
#include <EEPROM.h>
|
||||
#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);
|
||||
}
|
||||
|
||||
62
LARS.h
Normal file
62
LARS.h
Normal file
@ -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
|
||||
Loading…
Reference in New Issue
Block a user