library for stepping motor
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Jul 17 2022 05:06:56 by 1.7.2