From cff91e23b666c71eb8c2e85c83ef17b0570b46bd Mon Sep 17 00:00:00 2001 From: Lars H Date: Fri, 17 Nov 2017 15:02:33 +0100 Subject: [PATCH 1/2] cleaned, edit wave legnumbers --- LARS.cpp | 118 ++++++++++++++++++++------------------------------ LARS.h | 9 ++-- Octosnake.cpp | 4 +- Octosnake.h | 7 --- 4 files changed, 52 insertions(+), 86 deletions(-) diff --git a/LARS.cpp b/LARS.cpp index 025df2e..7b7ad25 100644 --- a/LARS.cpp +++ b/LARS.cpp @@ -1,5 +1,5 @@ -#include #include "LARS.h" +#include /* (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); -} - diff --git a/LARS.h b/LARS.h index 9dedd1a..100ee39 100644 --- a/LARS.h +++ b/LARS.h @@ -1,8 +1,10 @@ #ifndef LARS_h #define LARS_h +#include #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 diff --git a/Octosnake.cpp b/Octosnake.cpp index 3bffe5e..2eacd1e 100644 --- a/Octosnake.cpp +++ b/Octosnake.cpp @@ -1,7 +1,5 @@ -#include - #include "Octosnake.h" -//#include +#include Oscillator::Oscillator(){ _period = 2000; diff --git a/Octosnake.h b/Octosnake.h index fdf8e16..8797d97 100644 --- a/Octosnake.h +++ b/Octosnake.h @@ -1,17 +1,10 @@ -#include - #ifndef octosnake_h #define octosnake_h -#include -//#include - - #ifndef PI #define PI 3.14159 #endif - class Oscillator{ public: From c3783acf0d675be70474c009fe9d5d6d2231f00b Mon Sep 17 00:00:00 2001 From: Lars H Date: Fri, 17 Nov 2017 15:38:14 +0100 Subject: [PATCH 2/2] wave switch case fix --- LARS.cpp | 36 +++++++++++++++++++++++------------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/LARS.cpp b/LARS.cpp index 7b7ad25..d3ca42a 100644 --- a/LARS.cpp +++ b/LARS.cpp @@ -281,45 +281,55 @@ void LARS::hello() { } void LARS::wave(int legNumber){ + int amplitude[] = { 0,0,0,0,0,0,0,0}; + int offsetLeg[] = { 0,0,0,0,0,0,0,0}; int T = 350; - switch ( legNumer) { - case 1 : - int amplitude[] = {0, 60, 0, 0, 0, 0, 0, 0}; + switch ( legNumber) { + case 1 : { + amplitude[1] = 60; int offset[] = { 90-20 , 90-60, 90 , 90+70 , 90 , 90+90 , 90+140 , 90 }; + memcpy ( &offsetLeg, &offset, sizeof(offset) ); break; - case 2 : - int amplitude[] = {60, 0, 0, 0, 0, 0, 0, 0}; - int offset[] = { + } + case 2 : { + amplitude[0] = 60; + int offset[] = { 90-60 , 90-20, 90+70 , 90 , 90 , 90+90 , 90+140 , 90 }; + memcpy ( &offsetLeg, &offset, sizeof(offset) ); break; - case 3 : - int amplitude[] = {0, 0, 0, 0, 60, 0, 0, 0}; - int offset[] = { + } + case 3 : { + amplitude[4] = 60; + int offset[] = { 90-20 , 90-60, 90 , 90+70 , 90 , 90+90 , 90+140 , 90 }; + memcpy ( &offsetLeg, &offset, sizeof(offset) ); break; - case 4 : - int amplitude[] = {0, 0, 0, 0, 0, 60, 0, 0}; - int offset[] = { + } + case 4 : { + amplitude[5]= 60; + int offset[] = { 90-20 , 90-60, 90 , 90+70 , 90 , 90+90 , 90+140 , 90 }; + memcpy ( &offsetLeg, &offset, sizeof(offset) ); break; + } default : if (debug) Serial.println("Error, leg does not exist"); break; @@ -328,7 +338,7 @@ void LARS::wave(int legNumber){ float period[] = {T, T, T, T, T, T, T, T}; int phase[] = {0, 0, 0, 90, 0, 0, 0, 0}; - execute(3, period, amplitude, offset, phase); + execute(3, period, amplitude, offsetLeg, phase); delay (200); }