commit 3dbc6cf151e95ab107362fb9beb941c6083179fc Author: Lars H Date: Fri Nov 17 13:20:48 2017 +0100 Initial diff --git a/Oscillator.cpp b/Oscillator.cpp new file mode 100644 index 0000000..4d92273 --- /dev/null +++ b/Oscillator.cpp @@ -0,0 +1,129 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- GPL license +//-------------------------------------------------------------- +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" + #include +#endif +#include "Oscillator.h" +#include + +//-- This function returns true if another sample +//-- should be taken (i.e. the TS time has passed since +//-- the last sample was taken +bool Oscillator::next_sample() +{ + + //-- Read current time + _currentMillis = millis(); + + //-- Check if the timeout has passed + if(_currentMillis - _previousMillis > _TS) { + _previousMillis = _currentMillis; + + return true; + } + + return false; +} + +//-- Attach an oscillator to a servo +//-- Input: pin is the arduino pin were the servo +//-- is connected +void Oscillator::attach(int pin, bool rev) +{ + //-- If the oscillator is detached, attach it. + //Serial.println("PANIIIIIIIIIIIIIIIIIC"); +// Serial.println(!_servo.attached()); + //if(!_servo.attached()){ + + //-- Attach the servo and move it to the home position + _servo.attach(pin); + _servo.write(90); + + //-- Initialization of oscilaltor parameters + _TS=30; + _T=2000; + __N = _T/_TS; + _inc = 2*M_PI/__N; + + _previousMillis=0; + + //-- Default parameters + _A=45; + _phase=0; + _phase0=0; + _O=0; + _stop=false; + + //-- Reverse mode + _rev = rev; + //} + +} + +//-- Detach an oscillator from his servo +void Oscillator::detach() +{ + //-- If the oscillator is attached, detach it. + if(_servo.attached()) + _servo.detach(); + +} + +/*************************************/ +/* Set the oscillator period, in ms */ +/*************************************/ +void Oscillator::SetT(unsigned int T) +{ + //-- Assign the new period + _T=T; + + //-- Recalculate the parameters + __N = _T/_TS; + _inc = 2*M_PI/__N; +}; + +/*******************************/ +/* Manual set of the position */ +/******************************/ + +void Oscillator::SetPosition(int position) +{ + _servo.write(position+_trim); +}; + + +/*******************************************************************/ +/* This function should be periodically called */ +/* in order to maintain the oscillations. It calculates */ +/* if another sample should be taken and position the servo if so */ +/*******************************************************************/ +void Oscillator::refresh() +{ + + //-- Only When TS milliseconds have passed, the new sample is obtained + if (next_sample()) { + + //-- If the oscillator is not stopped, calculate the servo position + if (!_stop) { + //-- Sample the sine function and set the servo pos + _pos = round(_A * sin(_phase + _phase0) + _O); + if (_rev) _pos=-_pos; + Serial.println(_pos+90+_trim); + _servo.write(_pos+90+_trim); + } + + //-- Increment the phase + //-- It is always increased, even when the oscillator is stop + //-- so that the coordination is always kept + _phase = _phase + _inc; + + } +} diff --git a/Oscillator.h b/Oscillator.h new file mode 100644 index 0000000..0fc924e --- /dev/null +++ b/Oscillator.h @@ -0,0 +1,68 @@ +//-------------------------------------------------------------- +//-- Oscillator.pde +//-- Generate sinusoidal oscillations in the servos +//-------------------------------------------------------------- +//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 +//-- GPL license +//-------------------------------------------------------------- +#ifndef Oscillator_h +#define Oscillator_h + +#include + +//-- Macro for converting from degrees to radians +#ifndef DEG2RAD + #define DEG2RAD(g) ((g)*M_PI)/180 +#endif + +class Oscillator +{ + public: + Oscillator(int trim=0) {_trim=trim;}; + void attach(int pin, bool rev =false); + void detach(); + + void SetA(unsigned int A) {_A=A;}; + void SetO(unsigned int O) {_O=O;}; + void SetPh(double Ph) {_phase0=Ph;}; + void SetT(unsigned int T); + void SetTrim(int trim){_trim=trim;}; + int getTrim() {return _trim;}; + void SetPosition(int position); + void Stop() {_stop=true;}; + void Play() {_stop=false;}; + void Reset() {_phase=0;}; + void refresh(); + + private: + bool next_sample(); + + private: + //-- Servo that is attached to the oscillator + Servo _servo; + + //-- Oscillators parameters + unsigned int _A; //-- Amplitude (degrees) + unsigned int _O; //-- Offset (degrees) + unsigned int _T; //-- Period (miliseconds) + double _phase0; //-- Phase (radians) + + //-- Internal variables + int _pos; //-- Current servo pos + int _trim; //-- Calibration offset + double _phase; //-- Current phase + double _inc; //-- Increment of phase + double __N; //-- Number of samples + unsigned int _TS; //-- sampling period (ms) + + long _previousMillis; + long _currentMillis; + + //-- Oscillation mode. If true, the servo is stopped + bool _stop; + + //-- Reverse mode + bool _rev; +}; + +#endif \ No newline at end of file diff --git a/Zowi.cpp b/Zowi.cpp new file mode 100644 index 0000000..f64b40e --- /dev/null +++ b/Zowi.cpp @@ -0,0 +1,116 @@ +#include "Zowi.h" +#include + +void Zowi::init(int YL, int YR, int RL, int RR, bool load_calibration) { + servo[0].attach(YL); + servo[1].attach(YR); + servo[2].attach(RL); + servo[3].attach(RR); + + if (load_calibration) { + for (int i = 0; i < 4; i++) { + int servo_trim = EEPROM.read(i); + if (servo_trim > 128) servo_trim -= 256; + servo[i].SetTrim(servo_trim); + } + } + Serial.begin(115200); + Serial.println(" test"); + + for (int i = 0; i < 4; i++) servo_position[i] = 90; +} + +void Zowi::setTrims(int YL, int YR, int RL, int RR) { + servo[0].SetTrim(YL); + servo[1].SetTrim(YR); + servo[2].SetTrim(RL); + servo[3].SetTrim(RR); +} + +void Zowi::saveTrimsOnEEPROM() { + for (int i = 0; i < 4; i++) EEPROM.write(i, servo[i].getTrim()); +} + +void Zowi::moveServos(int time, int servo_target[]) { + if(time>10){ + for (int i = 0; i < 4; i++) increment[i] = ((servo_target[i]) - servo_position[i]) / (time / 10.0); + final_time = millis() + time; + + for (int iteration = 1; millis() < final_time; iteration++) { + partial_time = millis() + 10; + for (int i = 0; i < 4; i++) servo[i].SetPosition(servo_position[i] + (iteration * increment[i])); + while (millis() < partial_time); //pause + } + } + else{ + for (int i = 0; i < 4; i++) servo[i].SetPosition(servo_target[i]); + } + for (int i = 0; i < 4; i++) servo_position[i] = servo_target[i]; +} + +void Zowi::oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle=1){ + for (int i=0; i<4; i++) { + servo[i].SetO(O[i]); + servo[i].SetA(A[i]); + servo[i].SetT(T); + servo[i].SetPh(phase_diff[i]); + } + double ref=millis(); + for (double x=ref; x= 1) for(int i=0;i= 1) for(int i=0;i= 1) for(int i=0;i= 1) for(int i=0;i +#include +#include + + + +class Zowi +{ + public: + void init(int YL, int YR, int RL, int RR, bool load_calibration=0); + void setTrims(int YL, int YR, int RL, int RR); + void saveTrimsOnEEPROM(); + + void moveServos(int time, int servo_target[]); + void oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle); + + void walk(float steps=4, int T=1566); + void turnLeft(float steps, int T); + void turnRight(float steps, int T); + void backward(float steps, int T); + void jump(float steps=1, int T=1000); + + + private: + + Oscillator servo[4]; + int servo_trim[4]; + int servo_position[4]; + + unsigned long final_time; + unsigned long partial_time; + float increment[4]; +}; + +#endif + +