Initial
This commit is contained in:
Lars H
commit
3dbc6cf151
|
|
@ -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 <pins_arduino.h>
|
||||
#endif
|
||||
#include "Oscillator.h"
|
||||
#include <ESP32_Servo.h>
|
||||
|
||||
//-- 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;
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <ESP32_Servo.h>
|
||||
|
||||
//-- 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
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
#include "Zowi.h"
|
||||
#include <Oscillator.h>
|
||||
|
||||
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<T*cycle+ref; x=millis()){
|
||||
for (int i=0; i<4; i++){
|
||||
servo[i].refresh();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Zowi::walk(float steps, int T){
|
||||
int A[4]= {30, 30, 20, 20};
|
||||
int O[4] = {0, 0, 0, 0};
|
||||
double phase_diff[4] = {DEG2RAD(180), DEG2RAD(180), DEG2RAD(90), DEG2RAD(90)};
|
||||
|
||||
int cycles=(int)steps;
|
||||
|
||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
||||
}
|
||||
|
||||
void Zowi::backward(float steps, int T){
|
||||
int A[4]= {15, 15, 30, 30};
|
||||
int O[4] = {0, 0, 0, 0};
|
||||
double phase_diff[4] = {DEG2RAD(180), DEG2RAD(180), DEG2RAD(270), DEG2RAD(270)};
|
||||
|
||||
int cycles=(int)steps;
|
||||
|
||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
||||
}
|
||||
|
||||
void Zowi::turnLeft(float steps, int T){
|
||||
int A[4]= {10, 10, 25, 25};
|
||||
int O[4] = {0, 0, 0, 0};
|
||||
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(90)};
|
||||
|
||||
int cycles=(int)steps;
|
||||
|
||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
||||
}
|
||||
|
||||
void Zowi::turnRight(float steps, int T){
|
||||
int A[4]= {20, 20, 30, 10};
|
||||
int O[4] = {0, 0, 0, 0};
|
||||
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
|
||||
|
||||
int cycles=(int)steps;
|
||||
|
||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
||||
}
|
||||
|
||||
void Zowi::jump(float steps, int T){
|
||||
int up[]={90,90,150,30};
|
||||
moveServos(T,up);
|
||||
int down[]={90,90,90,90};
|
||||
moveServos(T,down);
|
||||
}
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#ifndef Zowi_h
|
||||
#define Zowi_h
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <ESP32_Servo.h>
|
||||
#include <Oscillator.h>
|
||||
#include <EEPROM.h>
|
||||
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue