cleaned, edit wave legnumbers
This commit is contained in:
Lars H
parent
f233845919
commit
cff91e23b6
118
LARS.cpp
118
LARS.cpp
|
|
@ -1,5 +1,5 @@
|
|||
#include <EEPROM.h>
|
||||
#include "LARS.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
/*
|
||||
(servo index, pin to attach pwm)
|
||||
|
|
@ -14,11 +14,6 @@
|
|||
|__| |__|
|
||||
|
||||
*/
|
||||
//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} {
|
||||
|
|
@ -55,23 +50,10 @@ void LARS::init() {
|
|||
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();
|
||||
|
|
@ -272,8 +254,8 @@ void LARS::pushUp(float steps, float T = 600) {
|
|||
}
|
||||
|
||||
void LARS::hello() {
|
||||
float sentado[] = {90 + 15, 90 - 15, 90 - 65, 90 + 65, 90 + 20, 90 - 20, 90 + 10, 90 - 10};
|
||||
moveServos(150, sentado);
|
||||
float seated[] = {90 + 15, 90 - 15, 90 - 65, 90 + 65, 90 + 20, 90 - 20, 90 + 10, 90 - 10};
|
||||
moveServos(150, seated);
|
||||
delay(200);
|
||||
|
||||
int z_amp = 40;
|
||||
|
|
@ -298,22 +280,54 @@ void LARS::hello() {
|
|||
|
||||
}
|
||||
|
||||
void LARS::wave(){
|
||||
void LARS::wave(int legNumber){
|
||||
|
||||
int z_amp = 40;
|
||||
int x_amp = 60;
|
||||
int T = 350;
|
||||
switch ( legNumer) {
|
||||
case 1 :
|
||||
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
|
||||
};
|
||||
break;
|
||||
case 2 :
|
||||
int amplitude[] = {60, 0, 0, 0, 0, 0, 0, 0};
|
||||
int offset[] = {
|
||||
90-60 , 90-20,
|
||||
90+70 , 90 ,
|
||||
90 , 90+90 ,
|
||||
90+140 , 90
|
||||
};
|
||||
break;
|
||||
case 3 :
|
||||
int amplitude[] = {0, 0, 0, 0, 60, 0, 0, 0};
|
||||
int offset[] = {
|
||||
90-20 , 90-60,
|
||||
90 , 90+70 ,
|
||||
90 , 90+90 ,
|
||||
90+140 , 90
|
||||
};
|
||||
break;
|
||||
case 4 :
|
||||
int amplitude[] = {0, 0, 0, 0, 0, 60, 0, 0};
|
||||
int offset[] = {
|
||||
90-20 , 90-60,
|
||||
90 , 90+70 ,
|
||||
90 , 90+90 ,
|
||||
90+140 , 90
|
||||
};
|
||||
break;
|
||||
default :
|
||||
if (debug) Serial.println("Error, leg does not exist");
|
||||
break;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -392,41 +406,3 @@ void LARS::execute(float steps, float period[8], int amplitude[8], int offset[8]
|
|||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
9
LARS.h
9
LARS.h
|
|
@ -1,8 +1,10 @@
|
|||
#ifndef LARS_h
|
||||
#define LARS_h
|
||||
#include <ESP32_Servo.h>
|
||||
#include "Octosnake.h"
|
||||
|
||||
// servo index to board_pins
|
||||
#define debug 0
|
||||
|
||||
#define FRONT_RIGHT_HIP 0
|
||||
#define FRONT_LEFT_HIP 1
|
||||
|
|
@ -28,7 +30,7 @@ class LARS {
|
|||
void pushUp(float steps, float period);
|
||||
void hello();
|
||||
void home();
|
||||
void wave();
|
||||
void wave(int legNumber);
|
||||
|
||||
void setServo(int id, float target);
|
||||
void reverseServo(int id);
|
||||
|
|
@ -37,8 +39,6 @@ class LARS {
|
|||
void setTrim(int index, int value) {
|
||||
trim[index] = value;
|
||||
}
|
||||
void storeTrim();
|
||||
void loadTrim();
|
||||
private:
|
||||
Oscillator oscillator[8];
|
||||
Servo servo[8];
|
||||
|
|
@ -55,8 +55,7 @@ class LARS {
|
|||
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
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
#include <ESP32_Servo.h>
|
||||
|
||||
#include "Octosnake.h"
|
||||
//#include <Servo.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
Oscillator::Oscillator(){
|
||||
_period = 2000;
|
||||
|
|
|
|||
|
|
@ -1,17 +1,10 @@
|
|||
#include <ESP32_Servo.h>
|
||||
|
||||
#ifndef octosnake_h
|
||||
#define octosnake_h
|
||||
|
||||
#include <Arduino.h>
|
||||
//#include <Servo.h>
|
||||
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.14159
|
||||
#endif
|
||||
|
||||
|
||||
class Oscillator{
|
||||
|
||||
public:
|
||||
|
|
|
|||
Loading…
Reference in New Issue