分享好人经验：改进EasyStepper，让步进电机的控制支持加速度

大连好人

我之前分享了一个 EasyStepper： http://www.geek-workshop.com/thread-3266-1-1.html

有同学提出，步进电机的控制最好带有加速度功能，所以呢，改进一下 EasyStepper，咱也支持加速度吧。

先说一下总体思路：

1、将整个转动按照百分比分为 stage1(0－25％)、stage2(26－75％)、stage3(75－100％) 三个阶段：
    stage1 = steps / 4; // 第一阶段的结束步
    stage2 = steps / 4 * 3; // 第二阶段的结束步
2、用指定的速度计算出每步的延时时长（stepTime），该时长作为第二阶段的延时使用；
3、用 stepTime * (acceleration + 1) 计算出起始延时时长（startTime）；
4、计算出第一阶段的时间递减步长：stage1StepTime = acceleration * stepTime / stage1；
5、计算出第三阶段的时间递增步长：stage3StepTime = acceleration * stepTime / (steps - stage2)；
6、在运行每步时，利用如下公式计算出每个阶段的延时时长：

    stage1：nextStepTime = startTime - stage1StepTime * stepsGone
    stage2：nextStepTime = stepTime
    stage3：nextStepTime = stepTime + stage3StepTime * (stepsGone - stage2 + 1)

    然后通过每次在当前时间上加 nextStepTime 获得下个步的执行时间。

来看 EasyStepper.h （基本上和上一个版本一致）

1. 
   
2. #ifndef EasyStepper_h
   
3. #define EasyStepper_h
   
4. 
   
5. #include <stdlib.h>
   
6. #if ARDUINO >= 100
   
7. #include <Arduino.h>
   
8. #else
   
9. #include <WProgram.h>
   
10. #include <wiring.h>
    
11. #endif
    
12. 
    
13. /**
    
14. * author [email][email protected][/email]
    
15. */
    
16. class EasyStepper
    
17. {
    
18.         public:
    
19.                
    
20.                 /**
    
21.                 * to create the EasyStepper
    
22.                 * parameters:
    
23.                 * stepPin the step pin
    
24.                 * directionPin the direction pin
    
25.                 * enablePin the enable pin
    
26.                 * directionPinsInverted if the value is true then invert HIGH/LOW value for direction pin
    
27.                 * enablePinsInverted if the value is true then invert HIGH/LOW value for enable pin
    
28.                 */
    
29.                 EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted);
    
30.                
    
31.                 /**
    
32.                 * to startup the EasyStepper
    
33.                 */
    
34.                 void startup();
    
35.                
    
36.                 /**
    
37.                 * to shutdown the EasyStepper, will release the enable pin to save power
    
38.                 */
    
39.                 void shutdown();
    
40.                
    
41.                 /**
    
42.                 * to rotate steps from current position with speed, the acceleration = 0
    
43.                 * speed the speed for rotation, the unit is steps per second
    
44.                 * steps the steps to go
    
45.                 */
    
46.                 void rotate(float speed, int steps);
    
47.                
    
48.                 /**
    
49.                 * to rotate steps from current position with speed
    
50.                 * speed the speed for rotation, the unit is steps per second
    
51.                 * steps the steps to go
    
52.                 * acceleration the acceleration, if it = 0, means no acceleration
    
53.                 */
    
54.                 void rotate(float speed, int steps, int acceleration);
    
55.                
    
56.                 /**
    
57.                 * to stop the stepper, the motor will stay at current position
    
58.                 */
    
59.                 void stop();
    
60.    
    
61.     /**
    
62.     * run, you must call this as frequently as possible, but at least once per minimum step interval,
    
63.     * preferably in your main loop.
    
64.     */
    
65.     void run();
    
66.    
    
67.     /**
    
68.     * if return true means the action is done
    
69.     */
    
70.     boolean isDone();
    
71.    
    
72.     /**
    
73.     * enable the debug mode?
    
74.     */
    
75.     void debugMode(boolean enabled);
    
76.                
    
77.         protected:
    
78.                
    
79.                 //
    
80.                
    
81.         private:
    
82.                
    
83.                 boolean debugModeFlag;
    
84.                
    
85.                 byte stepPin;
    
86.                 byte directionPin;
    
87.                 byte enablePin;
    
88.                
    
89.                 bool directionPinsInverted;
    
90.                 bool enablePinsInverted;
    
91.                
    
92.                 int acceleration;
    
93.                 int stage1; // the end step of the stage1
    
94.                 int stage2; // the end step of the stage2
    
95.                
    
96.                 int stepsToGo;
    
97.                 int stepsGone;
    
98.                
    
99.                 float stepTime;
    
100.                 float startTime;
     
101.                 float stage1StepTime;
     
102.                 float stage3StepTime;
     
103.                 unsigned long nextTimestamp;
     
104.                
     
105.                 boolean done;
     
106.                
     
107.                 void step();
     
108.                
     
109. };
     
110. 
     
111. #endif

复制代码

再来看 EasyStepper.cpp （代码里面有注释）

1. 
   
2. #include "EasyStepper.h"
   
3. 
   
4. EasyStepper::EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted)
   
5. {
   
6.         // save the parameters
   
7.         this->stepPin = stepPin;
   
8.         this->directionPin = directionPin;
   
9.         this->enablePin = enablePin;
   
10.         this->directionPinsInverted = directionPinsInverted;
    
11.         this->enablePinsInverted = enablePinsInverted;
    
12. }
    
13. 
    
14. void EasyStepper::startup()
    
15. {
    
16.         // set the pin mode
    
17.         pinMode(this->stepPin, OUTPUT);
    
18.         pinMode(this->directionPin, OUTPUT);
    
19.         pinMode(this->enablePin, OUTPUT);
    
20.         // enable the stepper
    
21.         digitalWrite(enablePin, HIGH ^ this->enablePinsInverted);
    
22.         // initialize the done to true
    
23.         this->done = true;
    
24. }
    
25. 
    
26. void EasyStepper::shutdown()
    
27. {
    
28.         // disable the stepper
    
29.         digitalWrite(enablePin, LOW ^ this->enablePinsInverted);
    
30. }
    
31. 
    
32. void EasyStepper::debugMode(boolean enabled)
    
33. {
    
34.         this->debugModeFlag = enabled;
    
35. }
    
36. 
    
37. void EasyStepper::rotate(float speed, int steps)
    
38. {
    
39.         this->rotate(speed, steps, 0);
    
40. }
    
41. 
    
42. void EasyStepper::rotate(float speed, int steps, int acceleration)
    
43. {
    
44.         // ignore the zero value
    
45.         if (speed != 0 && steps != 0)
    
46.         {
    
47.                 if (steps > 0)
    
48.                 {
    
49.                         // CW
    
50.                         digitalWrite(directionPin, HIGH ^ this->directionPinsInverted);
    
51.                 }
    
52.                 else if (steps < 0)
    
53.                 {
    
54.                         // CCW
    
55.                         digitalWrite(directionPin, LOW ^ this->directionPinsInverted);
    
56.                 }
    
57.                 // done flag
    
58.                 this->done = false;
    
59.                 // the steps to go
    
60.                 this->stepsToGo = abs(steps);
    
61.                 // the acceleration
    
62.                 this->acceleration = abs(acceleration);
    
63.                 if (this->stepsToGo <= 4)
    
64.                 {
    
65.                         this->acceleration = 0;
    
66.                 }
    
67.                 // change the speed to stepTime, micro seconds per step
    
68.                 this->stepTime = 1000.0 * 1000.0 / abs(speed);
    
69.                 // start time
    
70.                 this->startTime = this->stepTime * (this->acceleration + 1);
    
71.                 // stage1
    
72.                 this->stage1 = this->stepsToGo / 4;
    
73.                 this->stage1StepTime = (float) this->acceleration * this->stepTime / this->stage1;
    
74.                 // stage2
    
75.                 this->stage2 = this->stepsToGo / 4 * 3;
    
76.                 this->stage3StepTime = (float) this->acceleration * this->stepTime / (this->stepsToGo - this->stage2);
    
77.                 // the steps gone
    
78.                 this->stepsGone = 0;
    
79.                 // current timestamp
    
80.                 unsigned long time = micros();
    
81.                 if (this->debugModeFlag)
    
82.                 {
    
83.                         Serial.print("rotate: direction=");
    
84.                         Serial.print(steps > 0 ? "CW" : "CCW");
    
85.                         Serial.print(", stepsToGo=");
    
86.                         Serial.print(this->stepsToGo);
    
87.                         Serial.print(", acceleration=");
    
88.                         Serial.print(this->acceleration);
    
89.                         Serial.print(", startTime=");
    
90.                         Serial.print(this->startTime);
    
91.                         Serial.print(", stage1=");
    
92.                         Serial.print(this->stage1);
    
93.                         Serial.print(", stage1StepTime=");
    
94.                         Serial.print(this->stage1StepTime);
    
95.                         Serial.print(", stage2=");
    
96.                         Serial.print(this->stage2);
    
97.                         Serial.print(", stage3StepTime=");
    
98.                         Serial.print(this->stage3StepTime);
    
99.                         Serial.print(", stepTime=");
    
100.                         Serial.print(this->stepTime);
     
101.                         Serial.print(", currentTimestamp=");
     
102.                         Serial.println(time);
     
103.                 }
     
104.                 // call the step method to rotate the motor
     
105.                 this->step();
     
106.         }
     
107. }
     
108. 
     
109. void EasyStepper::stop()
     
110. {
     
111.         this->stepsToGo = 0;
     
112.         this->done = true;
     
113. }
     
114. 
     
115. void EasyStepper::run()
     
116. {
     
117.         // the current timestamp
     
118.         unsigned long time = micros();
     
119.         if (!this->done && time >= this->nextTimestamp)
     
120.         {
     
121.                 this->step();
     
122.         }
     
123.         if (this->debugModeFlag)
     
124.         {
     
125.                 Serial.print("currentTimeStamp=");
     
126.                 Serial.println(time);
     
127.         }
     
128. }
     
129. 
     
130. void EasyStepper::step()
     
131. {
     
132.         // are there some steps to rotate?
     
133.         if (this->stepsToGo > this->stepsGone)
     
134.         {
     
135.                 // HIGH value
     
136.                 digitalWrite(stepPin, HIGH);
     
137.                 // delay
     
138.                 delayMicroseconds(2);
     
139.                 // LOW value
     
140.                 digitalWrite(stepPin, LOW);
     
141.                 // increase the stepsGone
     
142.                 this->stepsGone++;
     
143.                 // default: stage2
     
144.                 float nextStepTime = this->stepTime;
     
145.                 if (this->acceleration != 0)
     
146.                 {
     
147.                         if (this->stepsGone < this->stage1)
     
148.                         {
     
149.                                 // stage1: down to stepTime from startTime
     
150.                                 nextStepTime = this->startTime - this->stage1StepTime * this->stepsGone;
     
151.                         }
     
152.                         else if (this->stepsGone >= this->stage2)
     
153.                         {
     
154.                                 // stage3: up to startTime from stepTime
     
155.                                 nextStepTime = this->stepTime + this->stage3StepTime * (this->stepsGone - this->stage2 + 1);
     
156.                         }
     
157.                 }
     
158.                 // current timestamp
     
159.                 unsigned long time = micros();
     
160.                 this->nextTimestamp = time + nextStepTime;
     
161.                 if (this->debugModeFlag)
     
162.                 {
     
163.                         Serial.print("step: stepsGone=");
     
164.                         Serial.print(this->stepsGone);
     
165.                         Serial.print(", currentTimestamp=");
     
166.                         Serial.print(time);
     
167.                         Serial.print(", nextTimestamp=");
     
168.                         Serial.print(this->nextTimestamp);
     
169.                         Serial.print(", nextStepTime=");
     
170.                         Serial.println(nextStepTime);
     
171.                 }
     
172.         }
     
173.         else
     
174.         {
     
175.                 this->done = true;
     
176.         }
     
177. }
     
178.    
     
179. boolean EasyStepper::isDone()
     
180. {
     
181.         return this->done;
     
182. }
     

复制代码

测试程序就不写了，和上一个版本差不多，在 rotate 方法增加一个加速度参数即可，如：

    float speed = 400.0;
    int steps = 200;
    int acceleration = 10;
    stepper1.rotate(speed, steps, acceleration);

有任何意见或建议，请随时与我联系，多谢。。。。

wasdpkj

好帖，这个很实用，算法不错

Malc

这个算法看的不是很明白，分成三个阶段的作用是？
加速度不断减小使速度曲线更光滑？
步进加减速要考虑两种情况，一是在给定距离内能达到指定速度，速度曲线是个梯形，
二是在给定距离内不能达到指定速度就要减速，否则会丢步，速度曲线是个三角形
不过一般的步进都会有个起步速度，这个速度是最大不丢步的启动速度，减速到这个速度可以直接刹车
实时加减速最大的问题单片机处理速度跟不上，因为要做大量浮点除法、开根号
推荐你看avr运用笔记446，这个是很经典的算法，实时计算的，accelStepper也是基于它的
最后，用上avr内部定时器和中断性能会提高很多，不然白浪费了这么好的资源 呵呵

Cola_DOG

请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！

deepsky

请教大侠这个程序可以驱动多个步进电机么？

JasmineL

deepsky 发表于 2013-4-24 09:22
请教大侠这个程序可以驱动多个步进电机么？

http://www.geek-workshop.com/thread-4319-1-1.html
这是我很多天研究以来的结果...

xiaojuren

Malc 发表于 2013-2-19 09:22
这个算法看的不是很明白，分成三个阶段的作用是？
加速度不断减小使速度曲线更光滑？
步进加减速要考虑两 ...

请问avr运用笔记446在哪

xiaojuren

Cola_DOG 发表于 2013-4-17 15:38
请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！

这个淘宝上有的

chqiyi

Cola_DOG 发表于 2013-4-17 15:38
请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！

好人在另一贴有说明，负极都接地

rick_hou

chqiyi 发表于 2013-6-13 23:22
好人在另一贴有说明，负极都接地

类似共阴或者共阳的解法吧。要么所有+都接5v，然后 - 接到管脚，或者反过来，我接驱动器的接法就是负极接管脚

Roger_Gao

请问，这个速度最高能达到多大啊？

yaoze权

请问楼主：EasyStepper.h这个库怎么下载？

阴霾の微笑

赞一个 谢谢楼主！

22881916

谢谢楼主了，，，可是不会添加.h和.c的

echo51u

楼主啊 怎么把你这个库添加到arduino里是