分享好人经验：改进EasyStepper，让步进电机的控制支持加速度
本帖最后由 大连好人 于 2013-2-6 17:41 编辑我之前分享了一个 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 （基本上和上一个版本一致）
#ifndef EasyStepper_h
#define EasyStepper_h
#include <stdlib.h>
#if ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#include <wiring.h>
#endif
/**
* author [email protected]
*/
class EasyStepper
{
public:
/**
* to create the EasyStepper
* parameters:
* stepPin the step pin
* directionPin the direction pin
* enablePin the enable pin
* directionPinsInverted if the value is true then invert HIGH/LOW value for direction pin
* enablePinsInverted if the value is true then invert HIGH/LOW value for enable pin
*/
EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted);
/**
* to startup the EasyStepper
*/
void startup();
/**
* to shutdown the EasyStepper, will release the enable pin to save power
*/
void shutdown();
/**
* to rotate steps from current position with speed, the acceleration = 0
* speed the speed for rotation, the unit is steps per second
* steps the steps to go
*/
void rotate(float speed, int steps);
/**
* to rotate steps from current position with speed
* speed the speed for rotation, the unit is steps per second
* steps the steps to go
* acceleration the acceleration, if it = 0, means no acceleration
*/
void rotate(float speed, int steps, int acceleration);
/**
* to stop the stepper, the motor will stay at current position
*/
void stop();
/**
* run, you must call this as frequently as possible, but at least once per minimum step interval,
* preferably in your main loop.
*/
void run();
/**
* if return true means the action is done
*/
boolean isDone();
/**
* enable the debug mode?
*/
void debugMode(boolean enabled);
protected:
//
private:
boolean debugModeFlag;
byte stepPin;
byte directionPin;
byte enablePin;
bool directionPinsInverted;
bool enablePinsInverted;
int acceleration;
int stage1; // the end step of the stage1
int stage2; // the end step of the stage2
int stepsToGo;
int stepsGone;
float stepTime;
float startTime;
float stage1StepTime;
float stage3StepTime;
unsigned long nextTimestamp;
boolean done;
void step();
};
#endif
再来看 EasyStepper.cpp （代码里面有注释）
#include "EasyStepper.h"
EasyStepper::EasyStepper(byte stepPin, byte directionPin, byte enablePin, bool directionPinsInverted, bool enablePinsInverted)
{
// save the parameters
this->stepPin = stepPin;
this->directionPin = directionPin;
this->enablePin = enablePin;
this->directionPinsInverted = directionPinsInverted;
this->enablePinsInverted = enablePinsInverted;
}
void EasyStepper::startup()
{
// set the pin mode
pinMode(this->stepPin, OUTPUT);
pinMode(this->directionPin, OUTPUT);
pinMode(this->enablePin, OUTPUT);
// enable the stepper
digitalWrite(enablePin, HIGH ^ this->enablePinsInverted);
// initialize the done to true
this->done = true;
}
void EasyStepper::shutdown()
{
// disable the stepper
digitalWrite(enablePin, LOW ^ this->enablePinsInverted);
}
void EasyStepper::debugMode(boolean enabled)
{
this->debugModeFlag = enabled;
}
void EasyStepper::rotate(float speed, int steps)
{
this->rotate(speed, steps, 0);
}
void EasyStepper::rotate(float speed, int steps, int acceleration)
{
// ignore the zero value
if (speed != 0 && steps != 0)
{
if (steps > 0)
{
// CW
digitalWrite(directionPin, HIGH ^ this->directionPinsInverted);
}
else if (steps < 0)
{
// CCW
digitalWrite(directionPin, LOW ^ this->directionPinsInverted);
}
// done flag
this->done = false;
// the steps to go
this->stepsToGo = abs(steps);
// the acceleration
this->acceleration = abs(acceleration);
if (this->stepsToGo <= 4)
{
this->acceleration = 0;
}
// change the speed to stepTime, micro seconds per step
this->stepTime = 1000.0 * 1000.0 / abs(speed);
// start time
this->startTime = this->stepTime * (this->acceleration + 1);
// stage1
this->stage1 = this->stepsToGo / 4;
this->stage1StepTime = (float) this->acceleration * this->stepTime / this->stage1;
// stage2
this->stage2 = this->stepsToGo / 4 * 3;
this->stage3StepTime = (float) this->acceleration * this->stepTime / (this->stepsToGo - this->stage2);
// the steps gone
this->stepsGone = 0;
// current timestamp
unsigned long time = micros();
if (this->debugModeFlag)
{
Serial.print("rotate: direction=");
Serial.print(steps > 0 ? "CW" : "CCW");
Serial.print(", stepsToGo=");
Serial.print(this->stepsToGo);
Serial.print(", acceleration=");
Serial.print(this->acceleration);
Serial.print(", startTime=");
Serial.print(this->startTime);
Serial.print(", stage1=");
Serial.print(this->stage1);
Serial.print(", stage1StepTime=");
Serial.print(this->stage1StepTime);
Serial.print(", stage2=");
Serial.print(this->stage2);
Serial.print(", stage3StepTime=");
Serial.print(this->stage3StepTime);
Serial.print(", stepTime=");
Serial.print(this->stepTime);
Serial.print(", currentTimestamp=");
Serial.println(time);
}
// call the step method to rotate the motor
this->step();
}
}
void EasyStepper::stop()
{
this->stepsToGo = 0;
this->done = true;
}
void EasyStepper::run()
{
// the current timestamp
unsigned long time = micros();
if (!this->done && time >= this->nextTimestamp)
{
this->step();
}
if (this->debugModeFlag)
{
Serial.print("currentTimeStamp=");
Serial.println(time);
}
}
void EasyStepper::step()
{
// are there some steps to rotate?
if (this->stepsToGo > this->stepsGone)
{
// HIGH value
digitalWrite(stepPin, HIGH);
// delay
delayMicroseconds(2);
// LOW value
digitalWrite(stepPin, LOW);
// increase the stepsGone
this->stepsGone++;
// default: stage2
float nextStepTime = this->stepTime;
if (this->acceleration != 0)
{
if (this->stepsGone < this->stage1)
{
// stage1: down to stepTime from startTime
nextStepTime = this->startTime - this->stage1StepTime * this->stepsGone;
}
else if (this->stepsGone >= this->stage2)
{
// stage3: up to startTime from stepTime
nextStepTime = this->stepTime + this->stage3StepTime * (this->stepsGone - this->stage2 + 1);
}
}
// current timestamp
unsigned long time = micros();
this->nextTimestamp = time + nextStepTime;
if (this->debugModeFlag)
{
Serial.print("step: stepsGone=");
Serial.print(this->stepsGone);
Serial.print(", currentTimestamp=");
Serial.print(time);
Serial.print(", nextTimestamp=");
Serial.print(this->nextTimestamp);
Serial.print(", nextStepTime=");
Serial.println(nextStepTime);
}
}
else
{
this->done = true;
}
}
boolean EasyStepper::isDone()
{
return this->done;
}
测试程序就不写了，和上一个版本差不多，在 rotate 方法增加一个加速度参数即可，如：
float speed = 400.0;
int steps = 200;
int acceleration = 10;
stepper1.rotate(speed, steps, acceleration);
有任何意见或建议，请随时与我联系，多谢。。。。
;P好帖，这个很实用，算法不错 这个算法看的不是很明白，分成三个阶段的作用是？
加速度不断减小使速度曲线更光滑？
步进加减速要考虑两种情况，一是在给定距离内能达到指定速度，速度曲线是个梯形，
二是在给定距离内不能达到指定速度就要减速，否则会丢步，速度曲线是个三角形
不过一般的步进都会有个起步速度，这个速度是最大不丢步的启动速度，减速到这个速度可以直接刹车
实时加减速最大的问题单片机处理速度跟不上，因为要做大量浮点除法、开根号
推荐你看avr运用笔记446，这个是很经典的算法，实时计算的，accelStepper也是基于它的
最后，用上avr内部定时器和中断性能会提高很多，不然白浪费了这么好的资源 呵呵 请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！ 请教大侠这个程序可以驱动多个步进电机么？ deepsky 发表于 2013-4-24 09:22 static/image/common/back.gif
请教大侠这个程序可以驱动多个步进电机么？
http://www.geek-workshop.com/thread-4319-1-1.html
这是我很多天研究以来的结果...
Malc 发表于 2013-2-19 09:22 static/image/common/back.gif
这个算法看的不是很明白，分成三个阶段的作用是？
加速度不断减小使速度曲线更光滑？
步进加减速要考虑两 ...
请问avr运用笔记446在哪 Cola_DOG 发表于 2013-4-17 15:38 static/image/common/back.gif
请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！
这个淘宝上有的 Cola_DOG 发表于 2013-4-17 15:38 static/image/common/back.gif
请问好人，对于arduino来说 TB6560上的 EN+/- CW+/- 和CLK+/-这六根线应该怎么接，急求？谢谢！
好人在另一贴有说明，负极都接地 chqiyi 发表于 2013-6-13 23:22 static/image/common/back.gif
好人在另一贴有说明，负极都接地
类似共阴或者共阳的解法吧。要么所有+都接5v，然后 - 接到管脚，或者反过来，我接驱动器的接法就是负极接管脚 请问，这个速度最高能达到多大啊？ 请问楼主：EasyStepper.h这个库怎么下载？ 赞一个 谢谢楼主！ 谢谢楼主了，，，可是不会添加.h和.c的 楼主啊 怎么把你这个库添加到arduino里是 :'(
页:
[1]