lbsadmin/LARS
1
0
Fork 0
Code Issues Pull Requests Releases Activity

cleaned, edit wave legnumbers

Browse Source
This commit is contained in:
Lars H 2017-11-17 15:02:33 +01:00
parent f233845919
commit cff91e23b6
4 changed files with 52 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
LARS.cpp
View File

@ -1,5 +1,5 @@
#include <EEPROM.h>
#include "LARS.h" #include "LARS.h"
#include <Arduino.h>
/* /*
(servo index, pin to attach pwm) (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} { 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_LEFT_LEG] = 6;
trim[BACK_RIGHT_LEG] = 5; 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++) { for (int i = 0; i < 8; i++) {
oscillator[i].start(); oscillator[i].start();
servo[i].attach(board_pins[i]); 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(); home();
@ -272,8 +254,8 @@ void LARS::pushUp(float steps, float T = 600) {
} }
void LARS::hello() { void LARS::hello() {
float sentado[] = {90 + 15, 90 - 15, 90 - 65, 90 + 65, 90 + 20, 90 - 20, 90 + 10, 90 - 10}; float seated[] = {90 + 15, 90 - 15, 90 - 65, 90 + 65, 90 + 20, 90 - 20, 90 + 10, 90 - 10};
moveServos(150, sentado); moveServos(150, seated);
delay(200); delay(200);
int z_amp = 40; int z_amp = 40;
@ -298,13 +280,11 @@ void LARS::hello() {
} }
void LARS::wave(){ void LARS::wave(int legNumber){
int z_amp = 40;
int x_amp = 60;
int T = 350; int T = 350;
switch ( legNumer) {
float period[] = {T, T, T, T, T, T, T, T}; case 1 :
int amplitude[] = {0, 60, 0, 0, 0, 0, 0, 0}; int amplitude[] = {0, 60, 0, 0, 0, 0, 0, 0};
int offset[] = { int offset[] = {
90-20 , 90-60, 90-20 , 90-60,
@ -312,8 +292,42 @@ void LARS::wave(){
90 , 90+90 , 90 , 90+90 ,
90+140 , 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 phase[] = {0, 0, 0, 90, 0, 0, 0, 0}; int phase[] = {0, 0, 0, 90, 0, 0, 0, 0};
execute(3, period, amplitude, offset, phase); execute(3, period, amplitude, offset, phase);
delay (200); delay (200);
} }
@ -392,41 +406,3 @@ void LARS::execute(float steps, float period[8], int amplitude[8], int offset[8]
yield(); 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
View File

@ -1,8 +1,10 @@
#ifndef LARS_h #ifndef LARS_h
#define LARS_h #define LARS_h
#include <ESP32_Servo.h>
#include "Octosnake.h" #include "Octosnake.h"
// servo index to board_pins // servo index to board_pins
#define debug 0
#define FRONT_RIGHT_HIP 0 #define FRONT_RIGHT_HIP 0
#define FRONT_LEFT_HIP 1 #define FRONT_LEFT_HIP 1
@ -28,7 +30,7 @@ class LARS {
void pushUp(float steps, float period); void pushUp(float steps, float period);
void hello(); void hello();
void home(); void home();
void wave(); void wave(int legNumber);
void setServo(int id, float target); void setServo(int id, float target);
void reverseServo(int id); void reverseServo(int id);
@ -37,8 +39,6 @@ class LARS {
void setTrim(int index, int value) { void setTrim(int index, int value) {
trim[index] = value; trim[index] = value;
} }
void storeTrim();
void loadTrim();
private: private:
Oscillator oscillator[8]; Oscillator oscillator[8];
Servo servo[8]; Servo servo[8];
@ -55,8 +55,7 @@ class LARS {
inline int angToUsec(float value) { inline int angToUsec(float value) {
return value / 180 * (MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) + MIN_PULSE_WIDTH; 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 #endif

4
Octosnake.cpp
View File

@ -1,7 +1,5 @@
#include <ESP32_Servo.h>
#include "Octosnake.h" #include "Octosnake.h"
//#include <Servo.h> #include <Arduino.h>
Oscillator::Oscillator(){ Oscillator::Oscillator(){
_period = 2000; _period = 2000;

7
Octosnake.h
View File

@ -1,17 +1,10 @@
#include <ESP32_Servo.h>
#ifndef octosnake_h #ifndef octosnake_h
#define octosnake_h #define octosnake_h
#include <Arduino.h>
//#include <Servo.h>
#ifndef PI #ifndef PI
#define PI 3.14159 #define PI 3.14159
#endif #endif
class Oscillator{ class Oscillator{
public: public: