极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 122431|回复: 42

分享好人经验:自己写个库,控制TB6560驱动步进电机

  [复制链接]
发表于 2013-1-24 17:20:33 | 显示全部楼层 |阅读模式
本帖最后由 大连好人 于 2013-2-6 17:20 编辑

更新提示:为 EasyStepper 增加了加速度功能,详情请见: http://www.geek-workshop.com/thread-3366-1-1.html

最近买了两个 42 步进电机和基于 TB6560 的步进电机驱动板,想学习一下步进电机到底是个什么东西。

研究了几天,发现现有库(如 Arduino 自带的 Stepper、Playground 里的 CustomeStepper、EasyDriver 推荐的 AccelStepper)都不是很好用,于是下决心自己写一个,顺便学习一下C++的代码。

先介绍一下这个步进电机驱动板(这个算广告吗? http://item.taobao.com/item.htm?id=12484846976 ):



最大支持 3A 输出,最多 16 细分,可设置静止电流和衰减。

之所以选择这个驱动板,是因为看到有很多二手的 TB6560 芯片出售,和汽车一样,保有量多的必定是好东西,呵呵。

输出就不说了,都一样。

输入端有3个:CLK、CW、EN,分表代表:

CLK:即 step 端口,给一个脉冲(HIGH-LOW)步进电机动一步;
CW:即 direction 端口,该板子定义为 LOW 为顺时针,HIGH 为逆时针;
EN:即 enable 端口,该板子定义为 LOW 为工作,HIGH 为脱机。

这样类型的接口,好像 Stepper 和 CustomeStepper 都不支持。

AccelStepper 可以用,但我发现由于它引入了加速度的概念(而且不能关闭),导致在多圈旋转时会出现不同步,具体问题懒得看,有兴趣的同学可以看看:  http://www.open.com.au/mikem/arduino/AccelStepper/index.html

其实,从原理上分析,让这个 TB6560 工作起来很简单,只要给脉冲就行了,如:

  1. void loop()
  2. {
  3.         digitalWrite(STEP_PIN, HIGH);
  4.         delay(1);
  5.         digitalWrite(STEP_PIN, LOW);
  6.         delay(10); // 修改这个参数可以控制速度
  7. }
复制代码


但问题是:如果使用 delay 方式,将导致不能同时使用多个步进电机,而且,对系统的其他代码也会有影响。

阅读了 CustomeStepper 的代码后,我的 EasyStepper 就来了。。。。

先看 EasyStepper.h 文件:


  1. #ifndef EasyStepper_h
  2. #define EasyStepper_h

  3. #include <stdlib.h>
  4. #if ARDUINO >= 100
  5. #include <Arduino.h>
  6. #else
  7. #include <WProgram.h>
  8. #include <wiring.h>
  9. #endif

  10. // These defs cause trouble on some versions of Arduino
  11. #undef round

  12. /**
  13. * author [email][email protected][/email]
  14. */
  15. class EasyStepper
  16. {
  17.         public:
  18.                
  19.                 /**
  20.                 * to create the EasyStepper
  21.                 * parameters:
  22.                 * stepPin the step pin
  23.                 * directionPin the direction pin
  24.                 * enablePin the enable pin
  25.                 * directionPinsInverted if the value is true then invert HIGH/LOW value for direction pin
  26.                 * enablePinsInverted if the value is true then invert HIGH/LOW value for enable pin
  27.                 */
  28.                 EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted);
  29.                
  30.                 /**
  31.                 * to startup the EasyStepper
  32.                 */
  33.                 void startup();
  34.                
  35.                 /**
  36.                 * to shutdown the EasyStepper, will release the enable pin to save power
  37.                 */
  38.                 void shutdown();
  39.                
  40.                 /**
  41.                 * to rotate steps from current position with speed
  42.                 * speed the speed for rotation, the unit is steps per second
  43.                 */
  44.                 void rotate(float speed, int steps);
  45.                
  46.                 /**
  47.                 * to stop the stepper, the motor will stay at current position
  48.                 */
  49.                 void stop();
  50.    
  51.     /**
  52.     * run, you must call this as frequently as possible, but at least once per minimum step interval,
  53.     * preferably in your main loop.
  54.     */
  55.     void run();
  56.    
  57.     /**
  58.     * if return true means the action is done
  59.     */
  60.     boolean isDone();
  61.    
  62.     /**
  63.     * enable the debug mode?
  64.     */
  65.     void debugMode(boolean enabled);
  66.                
  67.         protected:
  68.                
  69.                 //
  70.                
  71.         private:
  72.                
  73.                 boolean debugModeFlag;
  74.                
  75.                 byte stepPin;
  76.                 byte directionPin;
  77.                 byte enablePin;
  78.                
  79.                 bool directionPinsInverted;
  80.                 bool enablePinsInverted;
  81.                
  82.                 int stepsToGo;
  83.                 int stepsGone;
  84.                
  85.                 float stepTime;
  86.                 unsigned long nextTimestamp;
  87.                
  88.                 boolean done;
  89.                
  90.                 void step();
  91.                
  92. };

  93. #endif
复制代码


再来看 EasyStepper.cpp,代码里面有注释,应该容易理解的:


  1. #include "EasyStepper.h"

  2. EasyStepper::EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted)
  3. {
  4.         // save the parameters
  5.         this->stepPin = stepPin;
  6.         this->directionPin = directionPin;
  7.         this->enablePin = enablePin;
  8.         this->directionPinsInverted = directionPinsInverted;
  9.         this->enablePinsInverted = enablePinsInverted;
  10. }

  11. void EasyStepper::startup()
  12. {
  13.         // set the pin mode
  14.         pinMode(this->stepPin, OUTPUT);
  15.         pinMode(this->directionPin, OUTPUT);
  16.         pinMode(this->enablePin, OUTPUT);
  17.         // enable the stepper
  18.         digitalWrite(enablePin, HIGH ^ this->enablePinsInverted);
  19.         // initialize the done to true
  20.         this->done = true;
  21. }

  22. void EasyStepper::shutdown()
  23. {
  24.         // disable the stepper
  25.         digitalWrite(enablePin, LOW ^ this->enablePinsInverted);
  26. }

  27. void EasyStepper::debugMode(boolean enabled)
  28. {
  29.         this->debugModeFlag = enabled;
  30. }

  31. void EasyStepper::rotate(float speed, int steps)
  32. {
  33.         // ignore the zero value
  34.         if (speed != 0 && steps != 0)
  35.         {
  36.                 if (steps > 0)
  37.                 {
  38.                         // CW
  39.                         digitalWrite(directionPin, HIGH ^ this->directionPinsInverted);
  40.                         if (this->debugModeFlag)
  41.                         {
  42.                                 Serial.println("CW");
  43.                         }
  44.                 }
  45.                 else if (steps < 0)
  46.                 {
  47.                         // CCW
  48.                         digitalWrite(directionPin, LOW ^ this->directionPinsInverted);
  49.                         if (this->debugModeFlag)
  50.                         {
  51.                                 Serial.println("CCW");
  52.                         }
  53.                 }
  54.                 this->done = false;
  55.                 // the steps to go
  56.                 this->stepsToGo = abs(steps);
  57.                 // the steps gone
  58.                 this->stepsGone = 0;
  59.                 // change the speed to stepTime, micro seconds per step
  60.                 this->stepTime = 1000.0 * 1000.0 / abs(speed);
  61.                 // current timestamp
  62.                 unsigned long time = micros();
  63.                 this->nextTimestamp = time + this->stepTime;
  64.                 if (this->debugModeFlag)
  65.                 {
  66.                         Serial.print("stepsToGo=");
  67.                         Serial.print(this->stepsToGo);
  68.                         Serial.print(", stepTime=");
  69.                         Serial.print(this->stepTime);
  70.                         Serial.print(", currentTimestamp=");
  71.                         Serial.print(time);
  72.                         Serial.print(", nextTimestamp=");
  73.                         Serial.println(this->nextTimestamp);
  74.                 }
  75.                 // call the step method to rotate the motor
  76.                 this->step();
  77.         }
  78. }

  79. void EasyStepper::stop()
  80. {
  81.         this->stepsToGo = 0;
  82.         this->done = true;
  83. }

  84. void EasyStepper::run()
  85. {
  86.         // the current timestamp
  87.         unsigned long time = micros();
  88.         if (time >= this->nextTimestamp && !this->done)
  89.         {
  90.                 this->step();
  91.         }
  92.         if (this->debugModeFlag)
  93.         {
  94.                 Serial.print("currentTimestamp=");
  95.                 Serial.println(time);
  96.         }
  97. }

  98. void EasyStepper::step()
  99. {
  100.         // are there some steps to rotate?
  101.         if (this->stepsToGo > this->stepsGone)
  102.         {
  103.                 // HIGH value
  104.                 digitalWrite(stepPin, HIGH);
  105.                 // delay
  106.                 delayMicroseconds(2);
  107.                 // LOW value
  108.                 digitalWrite(stepPin, LOW);
  109.                 // increase the stepsGone
  110.                 this->stepsGone++;
  111.                 // current timestamp
  112.                 unsigned long time = micros();
  113.                 this->nextTimestamp = time + this->stepTime;
  114.                 if (this->debugModeFlag)
  115.                 {
  116.                         Serial.print("stepsGone=");
  117.                         Serial.print(stepsGone);
  118.                         Serial.print(", currentTimestamp=");
  119.                         Serial.print(time);
  120.                         Serial.print(", nextTimestamp=");
  121.                         Serial.println(this->nextTimestamp);
  122.                 }
  123.         }
  124.         else
  125.         {
  126.                 this->done = true;
  127.         }
  128. }
  129.    
  130. boolean EasyStepper::isDone()
  131. {
  132.         return this->done;
  133. }
复制代码


最后来看一个测试程序,逻辑很简单,来回转 10 次,然后停:


  1. #include <EasyStepper.h>

  2. // define the pins
  3. #define STEP_PIN 34
  4. #define DIR_PIN 35
  5. #define EN_PIN 36

  6. // define the inverted
  7. #define DIR_PIN_INVERTED true
  8. #define EN_PIN_INVERTED true

  9. // the EasyStepper instance
  10. EasyStepper stepper1(STEP_PIN, DIR_PIN, EN_PIN, DIR_PIN_INVERTED, EN_PIN_INVERTED);

  11. // the stepps to rotate
  12. int stepps = 200;

  13. // run times
  14. int times = 0;

  15. void setup()
  16. {
  17.   Serial.begin(9600);
  18.   stepper1.debugMode(false);
  19.   stepper1.startup();
  20.   stepper1.rotate(400.0, stepps);
  21. }

  22. void loop()
  23. {
  24.   if (times < 10)
  25.   {
  26.     stepper1.run();
  27.     if (stepper1.isDone())
  28.     {
  29.       // go back
  30.       stepps *= -1;
  31.       stepper1.rotate(400.0, stepps);
  32.       times++;
  33.     }
  34.   }
  35. }
复制代码


好人是个 Java 程序员,写 C++ 太吃力了,程序里面应该有 bug 或可以优化的地方,还请大家来指导,谢谢。。。。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x

评分

参与人数 1 +5 收起 理由
幻生幻灭 + 5 赞一个!

查看全部评分

回复

使用道具 举报

发表于 2013-1-24 17:49:00 | 显示全部楼层
呵呵 这个easystepper 对付300rpm以下的应该还不错,再高估计就丢步了
该有的基本都有了吧
步进电机最好还是有加减速控制的好,accelstepper用过,貌似有速度限制,到了一定速度后上不去,作者说4000HZ以上的频率就不可靠了
另,tb6050最高输入15khz,8细分下,步进只能达到562rpm。。。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-1-24 20:34:51 | 显示全部楼层
Malc 发表于 2013-1-24 17:49
呵呵 这个easystepper 对付300rpm以下的应该还不错,再高估计就丢步了
该有的基本都有了吧
步进电机最好还 ...

感谢点评。

300 rpm 的意思是一秒钟 5 转?嗯,对我能想到的项目是足够快了。

AccelStepper 也许是我不会用吧,总是不同步,就先用自己这个对付着了。。。。
回复 支持 反对

使用道具 举报

发表于 2013-1-24 21:15:56 | 显示全部楼层
大连好人 发表于 2013-1-24 20:34
感谢点评。

300 rpm 的意思是一秒钟 5 转?嗯,对我能想到的项目是足够快了。

accelstepper由于加减速,多电机联动是会有不同步的
你可以测试一下这段代码最高能运行多少转
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-1-25 09:19:21 | 显示全部楼层
Malc 发表于 2013-1-24 21:15
accelstepper由于加减速,多电机联动是会有不同步的
你可以测试一下这段代码最高能运行多少转

能详细和我说说为什么驱动步进电机最好是有加速度功能吗?如果有必要,我把我的程序也加上加速度。

另外,我说的 AccelStepper 不同步问题,不是指多个电机不同步,而是一个电机的问题。
例如,转一圈没问题,当需要转两圈以上时,就会出现停滞现象(每次都出现),即,运动完毕后不能回到出发点。不知道是为什么,我觉得和那个加速度有关系。
回复 支持 反对

使用道具 举报

发表于 2013-1-25 09:38:03 | 显示全部楼层
看看······················:)
回复 支持 反对

使用道具 举报

发表于 2013-1-26 10:52:23 | 显示全部楼层
大连好人 发表于 2013-1-25 09:19
能详细和我说说为什么驱动步进电机最好是有加速度功能吗?如果有必要,我把我的程序也加上加速度。

另 ...

步进电机的特性,高速时力矩会变小,如果突然高速启动或突然刹车,会造成丢步的
所以要用加减速控制,如果速度比较低,那就可以不用了
accelStepper我也用过,没发现它有这个问题
回复 支持 反对

使用道具 举报

发表于 2013-1-26 15:23:38 | 显示全部楼层
:)謝謝.可給我關於accelStepper的連結嗎?
回复 支持 反对

使用道具 举报

发表于 2013-1-27 21:56:42 | 显示全部楼层
期待完善更新
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-1-28 10:25:28 | 显示全部楼层
stanley 发表于 2013-1-26 15:23
謝謝.可給我關於accelStepper的連結嗎?

帖子里面有啊: http://www.open.com.au/mikem/arduino/AccelStepper/index.html
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-2-6 17:20:25 | 显示全部楼层
更新提示:为 EasyStepper 增加了加速度功能,详情请见: http://www.geek-workshop.com/thread-3366-1-1.html
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-2-6 17:22:52 | 显示全部楼层
Malc 发表于 2013-1-26 10:52
步进电机的特性,高速时力矩会变小,如果突然高速启动或突然刹车,会造成丢步的
所以要用加减速控制,如 ...

改进了一下,请您给 review 一下? http://www.geek-workshop.com/thread-3366-1-1.html
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-2-6 17:24:03 | 显示全部楼层
MicroCao 发表于 2013-1-27 21:56
期待完善更新

完善和更新了:为 EasyStepper 增加了加速度功能,详情请见: http://www.geek-workshop.com/thread-3366-1-1.html
回复 支持 反对

使用道具 举报

发表于 2013-3-22 00:22:56 | 显示全部楼层
正好需要。好东西,支持。
回复 支持 反对

使用道具 举报

发表于 2013-4-17 15:37:26 | 显示全部楼层
请问好人,对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接,急求?谢谢!
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 01:21 , Processed in 0.044899 second(s), 27 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表