Suteppa.cpp

00001 #include "Suteppa.h"
00002 Suteppa::Suteppa()
00003 {
00004 }
00005 void Suteppa::init(unsigned long allStep, void (*stepper)(int))
00006 {
00007     _allStep = allStep;
00008     _speed = 10000;
00009     _stepper = stepper;
00010     _smooth = false;
00011     timer.start();
00012 }
00013 void Suteppa::setSpeed(unsigned long speed)
00014 {
00015     _speed = speed;
00016     _initDiff = _initSpeed - _speed;
00017 }
00018 void Suteppa::beginSmooth(unsigned long step, unsigned long initSpeed)
00019 {
00020     _smoothStep = step;
00021     _initSpeed = initSpeed;
00022     _initDiff = _initSpeed - _speed;
00023     _smooth = true;
00024 }
00025 void Suteppa::beginSmooth()
00026 {
00027     _smoothStep = _defaultSmoothStep;
00028     _initDiff = _defaultInitSpeed - _speed;
00029     _smooth = true;
00030 }
00031 void Suteppa::setDefaultSmooth(unsigned long step, unsigned long initSpeed)
00032 {
00033     _defaultSmoothStep = step;
00034     _defaultInitSpeed = initSpeed;
00035 }
00036 
00037 bool Suteppa::tick()
00038 {
00039     if(_r_i >= _r_step) return false;
00040     unsigned long time = timer.read_us();
00041     if(time - _r_time < _r_interval) return true;
00042     _r_time = time;
00043     if(_r_smooth){
00044         _step += _r_direction;
00045         _stepper(_r_direction);
00046         float r;
00047         float p = _r_max;
00048         if (_r_i <= _r_smoothStep) {
00049             r = (_r_i / (float)_r_smoothStep);
00050             p = sin(r*M_PI/2) * _r_max;
00051         }else if (_r_step - _r_i <= _r_smoothStep + 1) {
00052             r = ((_r_step - 1) - _r_i) / (float)_r_smoothStep;
00053             p = sin(r*M_PI/2) * _r_max;
00054         }
00055         _r_interval = (1 - p) * _initDiff + _speed;
00056     }else{
00057         _step += _r_direction;
00058         _stepper(_r_direction);
00059         _r_interval = _speed;
00060     }
00061     _r_i ++;
00062     return true;
00063 }
00064 
00065 void Suteppa::rotate(int mode, long step, bool sync)
00066 {
00067     timer.reset();
00068     if(mode == 0){
00069         _rotateRelative(step, sync);
00070     }else if(mode == 1){
00071         _rotateAbsolute(step, false, sync);
00072     }else if(mode == 2){
00073         _rotateAbsolute(step, true, sync);
00074     }
00075 }
00076 
00077 void Suteppa::rotate(int mode, long step)
00078 {
00079     rotate(mode, step, true);
00080 }
00081 
00082 void Suteppa::_rotateRelative(long step, bool sync)
00083 {
00084     _r_smoothStep = _smoothStep;
00085     _r_direction = 1;
00086     float interval = 1 / (float)_r_smoothStep;
00087     _r_smooth = _smooth;
00088 
00089     if(step < 0){
00090         _r_direction = -1;
00091         step *= -1;
00092     }
00093     _r_step = step;
00094     if(step < _r_smoothStep*2.1) _r_smoothStep = step/2.1;
00095     if(_r_smoothStep < 1) _r_smooth = false;
00096     if(_initSpeed < _speed) _r_smooth = false;
00097     _r_max = interval * _r_smoothStep;
00098     _r_interval = 0;
00099     _r_time = 0;
00100     _r_i = 0;
00101     if(sync){
00102         while(tick());
00103     }
00104 }
00105 void Suteppa::_rotateAbsolute(long step, bool skip, bool sync)
00106 {
00107     if(skip){
00108         step = step%_allStep;
00109         _step = _step%_allStep;
00110         unsigned long diff = abs(step - _step);
00111         if(diff > _allStep / 2){
00112             if(_step < step){
00113                 step = -(_allStep - diff);
00114             }else{
00115                 step = (_allStep - diff);
00116             }
00117         }else{
00118             step = step - _step;
00119         }
00120     }else{
00121         step = step - _step;
00122     }
00123     _rotateRelative(step, sync);
00124 }
00125 void Suteppa::setHome()
00126 {
00127     _step = 0;
00128 }
00129 float Suteppa::sigmoid(float x)
00130 {
00131     return 1 / (1 + exp(-7 * x));
00132 }