Revision 0:9da9efc2119c, committed 2020-03-08

Comitter:

j_rocket_boy

Date:

Sun Mar 08 03:38:37 2020 +0000

Commit message:

Firm ware for Antenna rotator

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Suteppa.cpp	Sun Mar 08 03:38:37 2020 +0000
@@ -0,0 +1,132 @@
+#include "Suteppa.h"
+Suteppa::Suteppa()
+{
+}
+void Suteppa::init(unsigned long allStep, void (*stepper)(int))
+{
+    _allStep = allStep;
+    _speed = 10000;
+    _stepper = stepper;
+    _smooth = false;
+    timer.start();
+}
+void Suteppa::setSpeed(unsigned long speed)
+{
+    _speed = speed;
+    _initDiff = _initSpeed - _speed;
+}
+void Suteppa::beginSmooth(unsigned long step, unsigned long initSpeed)
+{
+    _smoothStep = step;
+    _initSpeed = initSpeed;
+    _initDiff = _initSpeed - _speed;
+    _smooth = true;
+}
+void Suteppa::beginSmooth()
+{
+    _smoothStep = _defaultSmoothStep;
+    _initDiff = _defaultInitSpeed - _speed;
+    _smooth = true;
+}
+void Suteppa::setDefaultSmooth(unsigned long step, unsigned long initSpeed)
+{
+    _defaultSmoothStep = step;
+    _defaultInitSpeed = initSpeed;
+}
+
+bool Suteppa::tick()
+{
+    if(_r_i >= _r_step) return false;
+    unsigned long time = timer.read_us();
+    if(time - _r_time < _r_interval) return true;
+    _r_time = time;
+    if(_r_smooth){
+        _step += _r_direction;
+        _stepper(_r_direction);
+        float r;
+        float p = _r_max;
+        if (_r_i <= _r_smoothStep) {
+            r = (_r_i / (float)_r_smoothStep);
+            p = sin(r*M_PI/2) * _r_max;
+        }else if (_r_step - _r_i <= _r_smoothStep + 1) {
+            r = ((_r_step - 1) - _r_i) / (float)_r_smoothStep;
+            p = sin(r*M_PI/2) * _r_max;
+        }
+        _r_interval = (1 - p) * _initDiff + _speed;
+    }else{
+        _step += _r_direction;
+        _stepper(_r_direction);
+        _r_interval = _speed;
+    }
+    _r_i ++;
+    return true;
+}
+
+void Suteppa::rotate(int mode, long step, bool sync)
+{
+    timer.reset();
+    if(mode == 0){
+        _rotateRelative(step, sync);
+    }else if(mode == 1){
+        _rotateAbsolute(step, false, sync);
+    }else if(mode == 2){
+        _rotateAbsolute(step, true, sync);
+    }
+}
+
+void Suteppa::rotate(int mode, long step)
+{
+    rotate(mode, step, true);
+}
+
+void Suteppa::_rotateRelative(long step, bool sync)
+{
+    _r_smoothStep = _smoothStep;
+    _r_direction = 1;
+    float interval = 1 / (float)_r_smoothStep;
+    _r_smooth = _smooth;
+
+    if(step < 0){
+        _r_direction = -1;
+        step *= -1;
+    }
+    _r_step = step;
+    if(step < _r_smoothStep*2.1) _r_smoothStep = step/2.1;
+    if(_r_smoothStep < 1) _r_smooth = false;
+    if(_initSpeed < _speed) _r_smooth = false;
+    _r_max = interval * _r_smoothStep;
+    _r_interval = 0;
+    _r_time = 0;
+    _r_i = 0;
+    if(sync){
+        while(tick());
+    }
+}
+void Suteppa::_rotateAbsolute(long step, bool skip, bool sync)
+{
+    if(skip){
+        step = step%_allStep;
+        _step = _step%_allStep;
+        unsigned long diff = abs(step - _step);
+        if(diff > _allStep / 2){
+            if(_step < step){
+                step = -(_allStep - diff);
+            }else{
+                step = (_allStep - diff);
+            }
+        }else{
+            step = step - _step;
+        }
+    }else{
+        step = step - _step;
+    }
+    _rotateRelative(step, sync);
+}
+void Suteppa::setHome()
+{
+    _step = 0;
+}
+float Suteppa::sigmoid(float x)
+{
+    return 1 / (1 + exp(-7 * x));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Suteppa.h	Sun Mar 08 03:38:37 2020 +0000
@@ -0,0 +1,72 @@
+#ifndef SUTEPPA_H
+#define SUTEPPA_H
+
+#include "mbed.h"
+#ifndef M_PI
+#define M_PI 3.14159265359
+#endif
+
+class Suteppa
+{
+    public:
+        Suteppa();
+        void init(unsigned long allStep, void (*stepper)(int));
+        void setSpeed(unsigned long speed);
+        
+        void setDefaultSmooth(unsigned long step, unsigned long initSpeed);
+
+        void beginSmooth(unsigned long step, unsigned long initSpeed);
+        void beginSmooth();
+
+        void endSmooth(){_smooth = false;};
+
+        long getStep(){return _step;};
+        long getStepAbsolute(){return _step%_allStep;};
+
+        unsigned long getSpeed(){return _speed;};
+
+        void rotate(int mode, long step, bool sync);
+        void rotate(int mode, long step);
+
+        void setHome();
+        bool tick();
+
+        static const int RELATIVE = 0;
+        static const int ABSOLUTE = 1;
+        static const int ABSOLUTE_SKIP = 2;
+    private:
+        void _delay(unsigned long time);
+        
+        void _rotateAbsolute(long step, bool skip, bool sync);
+        void _rotateRelative(long step, bool sync);
+
+        void (*_stepper)(int);
+        void (*_turner)(int);
+
+        bool _smooth;
+        
+        long _step;
+        
+        unsigned long _allStep;
+        unsigned long _speed;
+        unsigned long _initDiff;
+        unsigned long _smoothStep;
+        unsigned long _initSpeed;
+
+        unsigned long _defaultSmoothStep;
+        unsigned long _defaultInitSpeed;
+
+        int _r_direction;
+        unsigned long _r_step;
+        unsigned long _r_smoothStep;
+        bool _r_smooth;
+        float _r_max;
+        unsigned int _r_i;
+        unsigned long _r_time;
+        unsigned long _r_interval;
+        
+        float sigmoid(float x);
+        Timer timer;
+};
+
+#endif
\ No newline at end of file