diff --git a/Octosnake.cpp b/Octosnake.cpp
new file mode 100644
index 0000000..3bffe5e
--- /dev/null
+++ b/Octosnake.cpp
@@ -0,0 +1,87 @@
+#include <ESP32_Servo.h>
+
+#include "Octosnake.h"
+//#include <Servo.h>
+
+Oscillator::Oscillator(){
+    _period = 2000;
+    _amplitude = 50;
+    _phase = 0;
+    _offset = 0;
+    _stop = true;
+    _ref_time = millis();
+    _delta_time = 0;
+}
+
+float Oscillator::refresh(){
+    if (!_stop){
+        _delta_time = (millis()-_ref_time) % _period;
+        _output =   (float)_amplitude*sin(time_to_radians(_delta_time)
+                    + degrees_to_radians(_phase))
+                    + _offset;
+    }
+
+    return _output;
+}
+
+void Oscillator::reset(){
+    _ref_time = millis();
+}
+
+void Oscillator::start(){
+    reset();
+    _stop = false;
+}
+
+void Oscillator::start(unsigned long ref_time){
+    _ref_time = ref_time;
+    _stop = false;
+}
+
+void Oscillator::stop(){
+    _stop = true;
+}
+
+void Oscillator::setPeriod(int period){
+    _period = period;
+}
+
+void Oscillator::setAmplitude(int amplitude){
+    _amplitude = amplitude;
+}
+
+void Oscillator::setPhase(int phase){
+    _phase = phase;
+}
+
+void Oscillator::setOffset(int offset){
+    _offset = offset;
+}
+
+void Oscillator::setTime(unsigned long ref){
+    _ref_time = ref;
+}
+
+float Oscillator::getOutput(){
+    return _output;
+}
+
+unsigned long Oscillator::getTime(){
+    return _ref_time;
+}
+
+float Oscillator::getPhaseProgress(){
+    return ((float)_delta_time/_period) * 360;
+}
+
+float Oscillator::time_to_radians(double time){
+    return time*2*PI/_period;
+}
+
+float Oscillator::degrees_to_radians(float degrees){
+    return degrees*2*PI/360;
+}
+
+float Oscillator::degrees_to_time(float degrees){
+    return degrees*_period/360;
+}
diff --git a/Octosnake.h b/Octosnake.h
new file mode 100644
index 0000000..fdf8e16
--- /dev/null
+++ b/Octosnake.h
@@ -0,0 +1,47 @@
+#include <ESP32_Servo.h>
+
+#ifndef octosnake_h
+#define octosnake_h
+
+#include <Arduino.h>
+//#include <Servo.h>
+
+
+#ifndef PI
+  #define PI 3.14159
+#endif
+
+
+class Oscillator{
+
+    public:
+        Oscillator();
+        float refresh();
+        void reset();
+        void start();
+        void start(unsigned long ref_time);
+        void stop();
+        float time_to_radians(double time);
+        float degrees_to_radians(float degrees);
+        float degrees_to_time(float degrees);
+        void setPeriod(int period);
+        void setAmplitude(int amplitude);
+        void setPhase(int phase);
+        void setOffset(int offset);
+        void setTime(unsigned long ref);
+        float getOutput();
+        float getPhaseProgress();
+        unsigned long getTime();
+
+    private:
+        int _period;
+        int _amplitude;
+        int _phase;
+        int _offset;
+        float _output;
+        bool _stop;
+        unsigned long _ref_time = 0;
+        float _delta_time = 0;
+};
+
+#endif