Keegan Hu
/
whouse
dfd
Fork of puma_test by
Stepper_motor.cpp@1:6bf8f2422228, 2018-05-11 (annotated)
- Committer:
- glintligo
- Date:
- Fri May 11 06:55:13 2018 +0000
- Revision:
- 1:6bf8f2422228
- Parent:
- 0:2e00b81c9137
- Child:
- 2:ecf401a37aa3
fff
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Bilybill | 0:2e00b81c9137 | 1 | #include "Stepper_motor.h" |
glintligo | 1:6bf8f2422228 | 2 | /** |
glintligo | 1:6bf8f2422228 | 3 | * [Stepper_motor::Stepper_motor 构造函数 初始化一个步进电机对象] |
glintligo | 1:6bf8f2422228 | 4 | * @param _en [en引脚] |
glintligo | 1:6bf8f2422228 | 5 | * @param _stepPin [step引脚] |
glintligo | 1:6bf8f2422228 | 6 | * @param _dirPin [dir引脚] |
glintligo | 1:6bf8f2422228 | 7 | * @param Inter [行程开关引脚] |
glintligo | 1:6bf8f2422228 | 8 | * @param _ratio [减速比] |
glintligo | 1:6bf8f2422228 | 9 | * @param _micorstep [细分数] |
glintligo | 1:6bf8f2422228 | 10 | * @param _dir [初始方向] |
glintligo | 1:6bf8f2422228 | 11 | * @param _rec [电机的转一圈所需脉冲数(未减速前)] |
glintligo | 1:6bf8f2422228 | 12 | */ |
Bilybill | 0:2e00b81c9137 | 13 | Stepper_motor::Stepper_motor(PinName _en,PinName _stepPin,PinName _dirPin,PinName Inter,int _ratio,int _micorstep,int _dir,float _rec):Stepper(_en,_stepPin,_dirPin),InterruptIn(Inter) |
Bilybill | 0:2e00b81c9137 | 14 | { |
Bilybill | 0:2e00b81c9137 | 15 | ratio = _ratio; |
Bilybill | 0:2e00b81c9137 | 16 | microstep = _micorstep; |
Bilybill | 0:2e00b81c9137 | 17 | dir = _dir; |
glintligo | 1:6bf8f2422228 | 18 | Ori_rec = 360.0 / _rec; |
Bilybill | 0:2e00b81c9137 | 19 | pos_dir = 1; |
Bilybill | 0:2e00b81c9137 | 20 | this->enable(); |
Bilybill | 0:2e00b81c9137 | 21 | } |
glintligo | 1:6bf8f2422228 | 22 | /** |
glintligo | 1:6bf8f2422228 | 23 | * [Stepper_motor::Config 步进电机角度速度控制] |
glintligo | 1:6bf8f2422228 | 24 | * @param rec_rate [转速 单位deg/s] |
glintligo | 1:6bf8f2422228 | 25 | * @param rec [转动的角度 单位 °] |
glintligo | 1:6bf8f2422228 | 26 | * 若转动的角度为-1 则会一直转,此时rec_rate需大于0 |
glintligo | 1:6bf8f2422228 | 27 | * 除此之外,若转动角度或转速小于0,则电机反转 |
glintligo | 1:6bf8f2422228 | 28 | * 如在电机尚未停止前再次调用该函数,则会立刻开始新的电机运动 |
glintligo | 1:6bf8f2422228 | 29 | */ |
Bilybill | 0:2e00b81c9137 | 30 | void Stepper_motor::Config(float rec_rate,float rec) |
Bilybill | 0:2e00b81c9137 | 31 | { |
Bilybill | 0:2e00b81c9137 | 32 | if(rec < 0 && rec != -1) |
Bilybill | 0:2e00b81c9137 | 33 | { |
Bilybill | 0:2e00b81c9137 | 34 | rec_rate = 0 - rec_rate; |
Bilybill | 0:2e00b81c9137 | 35 | rec = 0 - rec; |
Bilybill | 0:2e00b81c9137 | 36 | } |
Bilybill | 0:2e00b81c9137 | 37 | long long int frequency = rec_rate * ratio * microstep / Ori_rec; |
Bilybill | 0:2e00b81c9137 | 38 | if(frequency < 0) |
Bilybill | 0:2e00b81c9137 | 39 | { |
Bilybill | 0:2e00b81c9137 | 40 | dir = 1 - pos_dir; |
Bilybill | 0:2e00b81c9137 | 41 | frequency = 0 - frequency; |
Bilybill | 0:2e00b81c9137 | 42 | } |
Bilybill | 0:2e00b81c9137 | 43 | else if(frequency == 0) |
Bilybill | 0:2e00b81c9137 | 44 | { |
Bilybill | 0:2e00b81c9137 | 45 | this->disable(); |
Bilybill | 0:2e00b81c9137 | 46 | return; |
Bilybill | 0:2e00b81c9137 | 47 | } |
Bilybill | 0:2e00b81c9137 | 48 | else if(frequency > 0) |
Bilybill | 0:2e00b81c9137 | 49 | { |
Bilybill | 0:2e00b81c9137 | 50 | dir = pos_dir; |
Bilybill | 0:2e00b81c9137 | 51 | } |
Bilybill | 0:2e00b81c9137 | 52 | long long int remain = rec * ratio * microstep / Ori_rec; |
Bilybill | 0:2e00b81c9137 | 53 | this->enable(); |
Bilybill | 0:2e00b81c9137 | 54 | this->step(dir,frequency,remain); |
Bilybill | 0:2e00b81c9137 | 55 | |
Bilybill | 0:2e00b81c9137 | 56 | } |
glintligo | 1:6bf8f2422228 | 57 | /** |
glintligo | 1:6bf8f2422228 | 58 | * [Stepper_motor::getDir 获取电机的转动方向dir] |
glintligo | 1:6bf8f2422228 | 59 | * @return [dir] |
glintligo | 1:6bf8f2422228 | 60 | */ |
Bilybill | 0:2e00b81c9137 | 61 | int Stepper_motor::getDir() |
Bilybill | 0:2e00b81c9137 | 62 | { |
Bilybill | 0:2e00b81c9137 | 63 | return this->dir; |
Bilybill | 0:2e00b81c9137 | 64 | } |
glintligo | 1:6bf8f2422228 | 65 | |
glintligo | 1:6bf8f2422228 | 66 | /** |
glintligo | 1:6bf8f2422228 | 67 | * [Stepper_motor::reConfig 步进电机反转] |
glintligo | 1:6bf8f2422228 | 68 | * @param _rec [反转的角度] |
glintligo | 1:6bf8f2422228 | 69 | * 速度为5°/s |
glintligo | 1:6bf8f2422228 | 70 | */ |
Bilybill | 0:2e00b81c9137 | 71 | void Stepper_motor::reConfig(float _rec) |
Bilybill | 0:2e00b81c9137 | 72 | { |
Bilybill | 0:2e00b81c9137 | 73 | dir = 1 - dir; |
Bilybill | 0:2e00b81c9137 | 74 | long long int frequency = 5 * ratio * microstep / Ori_rec; |
Bilybill | 0:2e00b81c9137 | 75 | long long int remain = _rec * ratio * microstep / Ori_rec; |
Bilybill | 0:2e00b81c9137 | 76 | this->enable(); |
Bilybill | 0:2e00b81c9137 | 77 | this->step(dir,frequency,remain); |
Bilybill | 0:2e00b81c9137 | 78 | } |
glintligo | 1:6bf8f2422228 | 79 | |
glintligo | 1:6bf8f2422228 | 80 | /** |
glintligo | 1:6bf8f2422228 | 81 | * [Stepper_motor::set 行程开关按下后的运动函数] |
glintligo | 1:6bf8f2422228 | 82 | */ |
Bilybill | 0:2e00b81c9137 | 83 | void Stepper_motor::set() |
Bilybill | 0:2e00b81c9137 | 84 | { |
glintligo | 1:6bf8f2422228 | 85 | this -> Config(0,0);//停止运动 |
Bilybill | 0:2e00b81c9137 | 86 | wait(0.01); |
Bilybill | 0:2e00b81c9137 | 87 | this -> enable(); |
Bilybill | 0:2e00b81c9137 | 88 | this -> Config(-5,7); |
Bilybill | 0:2e00b81c9137 | 89 | } |
glintligo | 1:6bf8f2422228 | 90 | |
glintligo | 1:6bf8f2422228 | 91 | /** [Stepper_motor::Init 步进电机初始化] */ |
Bilybill | 0:2e00b81c9137 | 92 | void Stepper_motor::Init() |
Bilybill | 0:2e00b81c9137 | 93 | { |
Bilybill | 0:2e00b81c9137 | 94 | this->Config(10,-1); |
Bilybill | 0:2e00b81c9137 | 95 | this->fall(this,&Stepper_motor::set); |
Bilybill | 0:2e00b81c9137 | 96 | } |