Daisuke Nakayama / Suteppa

library for stepping motor

Suteppa.cpp@0:9da9efc2119c, 2020-03-08 (annotated)

Committer:
j_rocket_boy
Date:
Sun Mar 08 03:38:37 2020 +0000
Revision:
0:9da9efc2119c
Firm ware for Antenna rotator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_rocket_boy 0:9da9efc2119c 1 #include "Suteppa.h"
j_rocket_boy 0:9da9efc2119c 2 Suteppa::Suteppa()
j_rocket_boy 0:9da9efc2119c 3 {
j_rocket_boy 0:9da9efc2119c 4 }
j_rocket_boy 0:9da9efc2119c 5 void Suteppa::init(unsigned long allStep, void (*stepper)(int))
j_rocket_boy 0:9da9efc2119c 6 {
j_rocket_boy 0:9da9efc2119c 7 _allStep = allStep;
j_rocket_boy 0:9da9efc2119c 8 _speed = 10000;
j_rocket_boy 0:9da9efc2119c 9 _stepper = stepper;
j_rocket_boy 0:9da9efc2119c 10 _smooth = false;
j_rocket_boy 0:9da9efc2119c 11 timer.start();
j_rocket_boy 0:9da9efc2119c 12 }
j_rocket_boy 0:9da9efc2119c 13 void Suteppa::setSpeed(unsigned long speed)
j_rocket_boy 0:9da9efc2119c 14 {
j_rocket_boy 0:9da9efc2119c 15 _speed = speed;
j_rocket_boy 0:9da9efc2119c 16 _initDiff = _initSpeed - _speed;
j_rocket_boy 0:9da9efc2119c 17 }
j_rocket_boy 0:9da9efc2119c 18 void Suteppa::beginSmooth(unsigned long step, unsigned long initSpeed)
j_rocket_boy 0:9da9efc2119c 19 {
j_rocket_boy 0:9da9efc2119c 20 _smoothStep = step;
j_rocket_boy 0:9da9efc2119c 21 _initSpeed = initSpeed;
j_rocket_boy 0:9da9efc2119c 22 _initDiff = _initSpeed - _speed;
j_rocket_boy 0:9da9efc2119c 23 _smooth = true;
j_rocket_boy 0:9da9efc2119c 24 }
j_rocket_boy 0:9da9efc2119c 25 void Suteppa::beginSmooth()
j_rocket_boy 0:9da9efc2119c 26 {
j_rocket_boy 0:9da9efc2119c 27 _smoothStep = _defaultSmoothStep;
j_rocket_boy 0:9da9efc2119c 28 _initDiff = _defaultInitSpeed - _speed;
j_rocket_boy 0:9da9efc2119c 29 _smooth = true;
j_rocket_boy 0:9da9efc2119c 30 }
j_rocket_boy 0:9da9efc2119c 31 void Suteppa::setDefaultSmooth(unsigned long step, unsigned long initSpeed)
j_rocket_boy 0:9da9efc2119c 32 {
j_rocket_boy 0:9da9efc2119c 33 _defaultSmoothStep = step;
j_rocket_boy 0:9da9efc2119c 34 _defaultInitSpeed = initSpeed;
j_rocket_boy 0:9da9efc2119c 35 }
j_rocket_boy 0:9da9efc2119c 36
j_rocket_boy 0:9da9efc2119c 37 bool Suteppa::tick()
j_rocket_boy 0:9da9efc2119c 38 {
j_rocket_boy 0:9da9efc2119c 39 if(_r_i >= _r_step) return false;
j_rocket_boy 0:9da9efc2119c 40 unsigned long time = timer.read_us();
j_rocket_boy 0:9da9efc2119c 41 if(time - _r_time < _r_interval) return true;
j_rocket_boy 0:9da9efc2119c 42 _r_time = time;
j_rocket_boy 0:9da9efc2119c 43 if(_r_smooth){
j_rocket_boy 0:9da9efc2119c 44 _step += _r_direction;
j_rocket_boy 0:9da9efc2119c 45 _stepper(_r_direction);
j_rocket_boy 0:9da9efc2119c 46 float r;
j_rocket_boy 0:9da9efc2119c 47 float p = _r_max;
j_rocket_boy 0:9da9efc2119c 48 if (_r_i <= _r_smoothStep) {
j_rocket_boy 0:9da9efc2119c 49 r = (_r_i / (float)_r_smoothStep);
j_rocket_boy 0:9da9efc2119c 50 p = sin(r*M_PI/2) * _r_max;
j_rocket_boy 0:9da9efc2119c 51 }else if (_r_step - _r_i <= _r_smoothStep + 1) {
j_rocket_boy 0:9da9efc2119c 52 r = ((_r_step - 1) - _r_i) / (float)_r_smoothStep;
j_rocket_boy 0:9da9efc2119c 53 p = sin(r*M_PI/2) * _r_max;
j_rocket_boy 0:9da9efc2119c 54 }
j_rocket_boy 0:9da9efc2119c 55 _r_interval = (1 - p) * _initDiff + _speed;
j_rocket_boy 0:9da9efc2119c 56 }else{
j_rocket_boy 0:9da9efc2119c 57 _step += _r_direction;
j_rocket_boy 0:9da9efc2119c 58 _stepper(_r_direction);
j_rocket_boy 0:9da9efc2119c 59 _r_interval = _speed;
j_rocket_boy 0:9da9efc2119c 60 }
j_rocket_boy 0:9da9efc2119c 61 _r_i ++;
j_rocket_boy 0:9da9efc2119c 62 return true;
j_rocket_boy 0:9da9efc2119c 63 }
j_rocket_boy 0:9da9efc2119c 64
j_rocket_boy 0:9da9efc2119c 65 void Suteppa::rotate(int mode, long step, bool sync)
j_rocket_boy 0:9da9efc2119c 66 {
j_rocket_boy 0:9da9efc2119c 67 timer.reset();
j_rocket_boy 0:9da9efc2119c 68 if(mode == 0){
j_rocket_boy 0:9da9efc2119c 69 _rotateRelative(step, sync);
j_rocket_boy 0:9da9efc2119c 70 }else if(mode == 1){
j_rocket_boy 0:9da9efc2119c 71 _rotateAbsolute(step, false, sync);
j_rocket_boy 0:9da9efc2119c 72 }else if(mode == 2){
j_rocket_boy 0:9da9efc2119c 73 _rotateAbsolute(step, true, sync);
j_rocket_boy 0:9da9efc2119c 74 }
j_rocket_boy 0:9da9efc2119c 75 }
j_rocket_boy 0:9da9efc2119c 76
j_rocket_boy 0:9da9efc2119c 77 void Suteppa::rotate(int mode, long step)
j_rocket_boy 0:9da9efc2119c 78 {
j_rocket_boy 0:9da9efc2119c 79 rotate(mode, step, true);
j_rocket_boy 0:9da9efc2119c 80 }
j_rocket_boy 0:9da9efc2119c 81
j_rocket_boy 0:9da9efc2119c 82 void Suteppa::_rotateRelative(long step, bool sync)
j_rocket_boy 0:9da9efc2119c 83 {
j_rocket_boy 0:9da9efc2119c 84 _r_smoothStep = _smoothStep;
j_rocket_boy 0:9da9efc2119c 85 _r_direction = 1;
j_rocket_boy 0:9da9efc2119c 86 float interval = 1 / (float)_r_smoothStep;
j_rocket_boy 0:9da9efc2119c 87 _r_smooth = _smooth;
j_rocket_boy 0:9da9efc2119c 88
j_rocket_boy 0:9da9efc2119c 89 if(step < 0){
j_rocket_boy 0:9da9efc2119c 90 _r_direction = -1;
j_rocket_boy 0:9da9efc2119c 91 step *= -1;
j_rocket_boy 0:9da9efc2119c 92 }
j_rocket_boy 0:9da9efc2119c 93 _r_step = step;
j_rocket_boy 0:9da9efc2119c 94 if(step < _r_smoothStep*2.1) _r_smoothStep = step/2.1;
j_rocket_boy 0:9da9efc2119c 95 if(_r_smoothStep < 1) _r_smooth = false;
j_rocket_boy 0:9da9efc2119c 96 if(_initSpeed < _speed) _r_smooth = false;
j_rocket_boy 0:9da9efc2119c 97 _r_max = interval * _r_smoothStep;
j_rocket_boy 0:9da9efc2119c 98 _r_interval = 0;
j_rocket_boy 0:9da9efc2119c 99 _r_time = 0;
j_rocket_boy 0:9da9efc2119c 100 _r_i = 0;
j_rocket_boy 0:9da9efc2119c 101 if(sync){
j_rocket_boy 0:9da9efc2119c 102 while(tick());
j_rocket_boy 0:9da9efc2119c 103 }
j_rocket_boy 0:9da9efc2119c 104 }
j_rocket_boy 0:9da9efc2119c 105 void Suteppa::_rotateAbsolute(long step, bool skip, bool sync)
j_rocket_boy 0:9da9efc2119c 106 {
j_rocket_boy 0:9da9efc2119c 107 if(skip){
j_rocket_boy 0:9da9efc2119c 108 step = step%_allStep;
j_rocket_boy 0:9da9efc2119c 109 _step = _step%_allStep;
j_rocket_boy 0:9da9efc2119c 110 unsigned long diff = abs(step - _step);
j_rocket_boy 0:9da9efc2119c 111 if(diff > _allStep / 2){
j_rocket_boy 0:9da9efc2119c 112 if(_step < step){
j_rocket_boy 0:9da9efc2119c 113 step = -(_allStep - diff);
j_rocket_boy 0:9da9efc2119c 114 }else{
j_rocket_boy 0:9da9efc2119c 115 step = (_allStep - diff);
j_rocket_boy 0:9da9efc2119c 116 }
j_rocket_boy 0:9da9efc2119c 117 }else{
j_rocket_boy 0:9da9efc2119c 118 step = step - _step;
j_rocket_boy 0:9da9efc2119c 119 }
j_rocket_boy 0:9da9efc2119c 120 }else{
j_rocket_boy 0:9da9efc2119c 121 step = step - _step;
j_rocket_boy 0:9da9efc2119c 122 }
j_rocket_boy 0:9da9efc2119c 123 _rotateRelative(step, sync);
j_rocket_boy 0:9da9efc2119c 124 }
j_rocket_boy 0:9da9efc2119c 125 void Suteppa::setHome()
j_rocket_boy 0:9da9efc2119c 126 {
j_rocket_boy 0:9da9efc2119c 127 _step = 0;
j_rocket_boy 0:9da9efc2119c 128 }
j_rocket_boy 0:9da9efc2119c 129 float Suteppa::sigmoid(float x)
j_rocket_boy 0:9da9efc2119c 130 {
j_rocket_boy 0:9da9efc2119c 131 return 1 / (1 + exp(-7 * x));
j_rocket_boy 0:9da9efc2119c 132 }