1
0
HIPPIE/Oscillator.cpp
2017-11-17 13:20:48 +01:00

130 lines
3.2 KiB
C++

//--------------------------------------------------------------
//-- 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;
}
}