具有恒定加速度的步进电机数学
Stepper motor math with constant acceleration
我需要编写一个程序来使用 atmega328P 和 A4988 芯片控制步进电机。
我一直在寻找合适的库，但到目前为止我还没有找到合适的。
我需要一个转盘来转向预示教的绝对位置。我认为有问题的电机每转 20000 步，微步为 1/16。当位置计数器达到 20000 时，必须将其设置回 0，反之亦然。所以从 19999 到 1 可以双向进行。
我想要步进电机加减速直线。这是我不完全知道该怎么做的一件事。
我想使用 arduino IDE 的 micros() 定时器功能来对步进引脚上的脉冲计时。
我可以计算出我必须移动多少脉冲，我可以increment/decrement以固定的间隔速度，但我在计算何时开始减速时遇到困难。
如果我调整速度，每走一步后，加速度就不是线性的了。我试图用以下公式计算制动距离：pulses = VV / 2。我从基本运动和行程公式中得出。 S(t) = S0 + V0 * t - 0.5A*t^2。我为 A 取 1，为 T 取 1。每次我 inc/dec 速度为 1。所以我得到 S(1) = V^2 - 0.5 * V^2 = 0. * V^2 或 V^ 2 / 2.
增加速度与脉冲不同步，我不知道如何解决这个问题。
我需要一些指点。
背景：
步进器将控制模型铁路转盘。转盘将正确连接到电机的轴上。
控制器将有 4 个按钮。
- manual/automatic 按钮。
- 顺时针
- 逆时针
- 店铺位置
在自动模式下，电机可以通过按 CW 和 CCW 从一个示教位置切换到另一个位置。还必须能够在转盘还在转动的时候接受新的位置。
在手动模式下，我可以使用相同的 CW 和 CCW 按钮手动巡航转盘。当我按住一个按钮时，电机需要加速到预设的最大速度，当我松开按钮时，电机需要减速到0。
使用存储按钮，我可以将当前位置保存在 20 个保留位置之一中。
控制器第一次上电时，会以电机当前物理位置为虚拟零点。每次转盘停止时，它的当前位置将被存储在 EEPROM 中。控制器关闭后，转盘不会因外力移动而明显移动。
编辑评论+答案
我已经做了一个简单的状态机。
我没有使用任何库来控制步进器，我目前使用 micros() 计时器来设置步进。
宏 REPEAT_MS 和 END_REPEAT 之间的所有代码在 us
中以给定的间隔运行
void setSteps()
{
uint8_t speedVar = 255 - speed ;
REPEAT_US( speedVar ) ;
if( enabled )
{
if( !digitalRead( dirPin ) )
{
if( ++position == maxPos+1 ) position = 1 ; // verify this code if positions work well
digitalWrite( stepPin, HIGH ) ;
digitalWrite( stepPin, LOW ) ; // slow enough for mininum pulse duration
}
else
{
//if( --position == 0 ) position = maxPos ; // currently run 1 direction
}
}
Serial.print("position: ");
Serial.println( position ) ;
else
{
digitalWrite( stepPin, state ) ;// may become port manipulated
return ;
}
END_REPEAT
}
控制速度：
void manageSpeed()
{
REPEAT_MS( ACCELERATION_FACTOR ) ; // separate fixed timing for constant acceleration and deceleration
static uint8_t speedPrev = 255 ;
if( speedSetpoint > speed ) speed ++ ;
if( speedSetpoint < speed ) speed -- ;
if( speed == 0 ) enabled = false ; // if speed reaches 0, stop sending pulses
if( speedPrev != speed ) {
speedPrev = speed ;
Serial.print("SPEED :") ;
Serial.println( speed ) ;
}
END_REPEAT
}
和不太重要的使用宏（它们工作正常，经过良好测试）
#define REPEAT_MS(x) { \
static uint32_t previousTime ;\
uint32_t currentTime = millis() ;\
if( currentTime - previousTime >= x ) {\
previousTime = currentTime ;
// code to be repeated goes between these 2 macros
#define REPEAT_US(x) { \
static uint32_t previousTime ;\
uint32_t currentTime = micros() ;\
if( currentTime - previousTime >= x ) {\
previousTime = currentTime ;
// code to be repeated goes between these 2 macros
#define END_REPEAT } \
}
我将仔细研究并尝试实现梯形控制器并尝试实现建议的公式。
第二次编辑：
我目前有一些工作代码，但有一件事我无法理解。
目前我是每100ms调速1。1是加速因子。总加速时间是 3500ms 因为 35 是我的最大速度。
这是加速时的输出。我不得不微调一下步长间隔。
速度：34
上次：100 总时间：3400
距离：6853
总排名：1147
SPEED :35
Last Time: 100 total time: 3500
distanceToGo: 6784
total position: 1216
cruising
这个 1216 应该是 1225。P(t) = 0.5 * 1 * 35^2 = 1225 个脉冲。然而，它已经足够接近了。
比减速：
braking
SPEED :34
Last Time: 27 total time: 11400
distanceToGo: 1207
total position: 6793
SPEED :33
Last Time: 100 total time: 11500
distanceToGo: 1137
total position: 6863
...
SPEED :0
Last Time: 100 total time: 14800
distanceToGo: -8
total position: 8008
Last Time: 1 total time: 14801
distanceToGo: -8
total position: 8008
我有 8 的超调，这是可以接受的。但是我没有得到的是以下内容。现在step时间间隔的计算是
REPEAT_US( 47950 / speed ) ;
47950除了不完美之外，也是无法估量的。我通过反复试验找到了它，无法解释为什么会是这样。
位置控制器方法大致如何工作？
据我了解，您需要的是一个位置控制器，具有从目标位置到目标位置的定义加速度和减速度。最简单的运动控制形式是“梯形”控制器——即具有线性加速度、恒定最大值和线性减速度的控制器；如果您绘制速度随时间变化的曲线，则形成一个梯形。当位置变化较小时，可能达不到最大速度，此时运动呈三角形。
一个合适的 class 来实现这个可能有：
- 定义梯形的参数（加速度、最大速度、减速度），
- 一个位置范围的参数（在你的情况下是0到20000），这样当设置了一个特定的位置位置设定点时，控制器可以设置最短运动的方向，
- 用于定义目标位置的设定点函数，
- 确定位置误差的更新函数（当前位置与所需位置之间的差异，并发出所需的步数以将误差归零）。此函数只需要足够频繁地调用以确保平滑运动，理想情况下它一次发出的步数不超过一步
这样的 class 的单位可能是步数 (steps/s、steps/s2) 或度数，或其他.我建议使用步进 - 它会提供最好的控制和最低的 CPU 负载，并且它是独立于系统的 - 例如，如果你正在驾驶线性执行器或车辆或有额外的传动装置，度数没有多大意义在任何情况下，您都需要进行额外的计算以将其转换为现实世界的系统域。
棘手的部分可能是确定何时停止加速以及何时开始减速。在这个应用程序中，我建议您保持简单，不要让 set-point/target 位置在一个动作完成之前改变。这使得计算更简单，因为当您规划梯形时，速度将始终为零。同样在这个应用中也不需要支持加速度到速度的动态变化。
所以我们需要的基本方程是：
p(t) = v0t + 0.5at2
使得任何特定时间的位置 t - 即 p(t) - 是初始速度加上一半加速度的平方。
对于梯形相位 (1)、(2)、(3)：
^ _________________
| /| |\
Vel | / | | \
| / | | \
| /(1)| (2) |(3)\
------------------------------>
time
等式是：
- (1) p(t) = 0.5a加速度 。 t2
- (2) p(t) = vmax 。 t
- (3) p(t) = vmax 。 t - 0.5a减速。 t2
注意到在每个阶段t被重置，并且p(t)是相对于阶段开始的位置（增量）。
但是对于短距离来说，运动是三角形的，不会达到vmax。那么你必须在停止距离小于或等于剩余距离时开始减速。
这里还有一个有用的方程式：
- (4) v = 在
您可以在阶段 (1) (v = at) 和阶段 (2) (*v = vmax) 来确定“停止距离” (v2 / 2a) 来确定阶段 (3) 的开始。
因此，我建议将更新功能实现为每个阶段的状态机。
下面的实现已经在 PC 上的测试工具 运行ning 中进行了数值测试。它假定使用标准 Arduino Library Stepper class 来执行实际步进，并且 class 引用 Stepper 对象，因此您可以控制多个电机。我还没有（还）在真实硬件上测试过它。我在 PC 上将 millis() 函数和 Stepper class 存根到 运行。
class cTrapezoidStepper
{
public:
cTrapezoidStepper( Stepper& stepper, int32_t accel, int32_t vmax, int32_t decel, int32_t range ) :
m_stepper( stepper ),
m_accel( accel ),
m_vmax( vmax ),
m_vpeak( 0 ),
m_decel( decel ),
m_range( range ),
m_start_time( 0 ),
m_current_pos( 0 ),
m_target_delta( 0 ),
m_direction( 0 ),
m_distance_moved( 0 ),
m_trap_phase( STOP )
{
}
bool isMoving()
{
return m_trap_phase != 0 ;
}
int32_t setTargetPos( int32_t target )
{
if( m_trap_phase == STOP &&
target != m_current_pos )
{
// Determine number of steps and direction to target
m_target_delta = target % m_range - m_current_pos ;
if( abs( m_target_delta ) > m_range / 2 )
{
m_target_delta -= m_range ;
}
m_direction = 1 ;
if( m_target_delta < 0 )
{
m_target_delta = -m_target_delta ;
m_direction = -1 ;
}
// Init motion
m_start_time = tick() ;
m_distance_moved = 0 ;
m_trap_phase = ACCEL ;
}
return m_target_delta ;
}
int32_t update()
{
int32_t t = tick() - m_start_time ;
int32_t distance_to_move = m_distance_moved ;
int32_t distance_to_target = m_target_delta - m_distance_moved ;
int32_t current_velocity = 0 ;
int32_t stopping_distance = 0 ;
switch( m_trap_phase )
{
case ACCEL :
{
current_velocity = (m_accel * t) / ONE_SECOND ;
stopping_distance = (current_velocity * current_velocity) / (2 * m_decel) ;
m_vpeak = current_velocity ;
if( distance_to_target <= stopping_distance )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = DECEL ;
}
else if( current_velocity >= m_vmax )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = CONSTANT ;
}
else
{
distance_to_move = m_accel * (t * t) / (2 * ONE_SECOND * ONE_SECOND) ;
}
}
break ;
case CONSTANT :
{
m_vpeak = m_vmax ;
current_velocity = m_vmax ;
stopping_distance = (current_velocity * current_velocity) / (2 * m_decel) ;
if( distance_to_target <= stopping_distance )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = DECEL ;
}
else
{
distance_to_move = (t * m_vmax) / ONE_SECOND ;
}
}
break ;
case DECEL :
{
current_velocity = m_vpeak - (m_decel * t) / ONE_SECOND ;
if( distance_to_target <= 0 )
{
m_trap_phase = STOP ;
}
else
{
distance_to_move = (m_vpeak * t) / ONE_SECOND -
(m_decel * (t * t) / (2 * ONE_SECOND * ONE_SECOND)) ;
}
}
break ;
default :
{
distance_to_move = m_distance_moved ;
}
break ;
}
// Do steps
int32_t steps = (distance_to_move - m_distance_moved) * m_direction ;
m_stepper.step( steps ) ;
// Update position
m_distance_moved = distance_to_move ;
m_current_pos += steps ;
m_current_pos %= m_range ;
return m_current_pos ;
}
private :
static const int32_t ONE_SECOND = 100 ;
Stepper& m_stepper ;
int32_t m_accel ;
int32_t m_vmax ;
int32_t m_vpeak ;
int32_t m_decel ;
int32_t m_range ;
unsigned long m_start_time ;
int32_t m_current_pos ;
int32_t m_target_delta ;
int32_t m_direction ;
int32_t m_distance_moved ;
enum
{
STOP,
ACCEL,
CONSTANT,
DECEL
} m_trap_phase ;
int32_t tick(){ return millis() / (1000 / ONE_SECOND) ; }
};
测试（Windows 不是 Arduino 代码，也不是你如何在 Sketch 中使用它 loop()）看起来像这样：
int main()
{
Stepper dummy ;
cTrapezoidStepper turntable( dummy, 100, 600, 150, 20000 ) ;
turntable.setTargetPos( 5000 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 2500 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 6000 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 0 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
}
所以
cTrapezoidStepper turntable( dummy, 100, 600, 150, 20000 ) ;
实例化一个控制器：
- 加速度 100 steps/s2
- 最大速度600steps/s
- 加速度150steps/s2
- 步长范围 0 到 19999 步。
当您设置位置时，它会选择最短的方向到达目标。
测试依次移动到位置 5000、2500、6000 然后 0，每次移动之间有一个延迟。药水配置文件如下：
请注意，延迟期间的速度并未显示为平坦 - 这是测试输出的人工制品以及精确减速计算使得在达到目标位置时它不完全为零的事实（尽管它确实停止了，如从位置配置文件。我确信可以修复，但我也确信它在这个应用程序中可能可以忽略不计。它停止在大约 38 steps/s 秒或大约 0.1 RPM。
另请注意，第二个移动是三角形的，因为距离太短，无法达到最大速度。
要在 Sketch 中执行类似的测试，您可能需要 loop() 如：
void loop()
{
int32_t positions[] = { 19000, 4000, 2500, 6000, 0 } ;
static int test = -1 ;
if( !turntable.isMoving() )
{
test = (test + 1) % (sizeof(positions) / sizeof(*positions)) ;
turntable.setTargetPos( positions[test] ) ;
}
else
{
turntable.update() ;
}
}
请注意我实现的成员函数 cTrapezoidStepper::tick() 以将计时分辨率降低到 100 毫秒间隔。这对于平滑控制来说应该没问题，但对于防止算术溢出是必要的（如果你使用非常低的加速度你可能仍然会遇到 - 如果你想让它更通用，那部分可以改进）。
我需要编写一个程序来使用 atmega328P 和 A4988 芯片控制步进电机。 我一直在寻找合适的库，但到目前为止我还没有找到合适的。
我需要一个转盘来转向预示教的绝对位置。我认为有问题的电机每转 20000 步，微步为 1/16。当位置计数器达到 20000 时，必须将其设置回 0，反之亦然。所以从 19999 到 1 可以双向进行。
我想要步进电机加减速直线。这是我不完全知道该怎么做的一件事。
我想使用 arduino IDE 的 micros() 定时器功能来对步进引脚上的脉冲计时。
我可以计算出我必须移动多少脉冲，我可以increment/decrement以固定的间隔速度，但我在计算何时开始减速时遇到困难。
如果我调整速度，每走一步后，加速度就不是线性的了。我试图用以下公式计算制动距离：pulses = VV / 2。我从基本运动和行程公式中得出。 S(t) = S0 + V0 * t - 0.5A*t^2。我为 A 取 1，为 T 取 1。每次我 inc/dec 速度为 1。所以我得到 S(1) = V^2 - 0.5 * V^2 = 0. * V^2 或 V^ 2 / 2.
增加速度与脉冲不同步，我不知道如何解决这个问题。
我需要一些指点。
背景： 步进器将控制模型铁路转盘。转盘将正确连接到电机的轴上。 控制器将有 4 个按钮。
- manual/automatic 按钮。
- 顺时针
- 逆时针
- 店铺位置
在自动模式下，电机可以通过按 CW 和 CCW 从一个示教位置切换到另一个位置。还必须能够在转盘还在转动的时候接受新的位置。
在手动模式下，我可以使用相同的 CW 和 CCW 按钮手动巡航转盘。当我按住一个按钮时，电机需要加速到预设的最大速度，当我松开按钮时，电机需要减速到0。
使用存储按钮，我可以将当前位置保存在 20 个保留位置之一中。
控制器第一次上电时，会以电机当前物理位置为虚拟零点。每次转盘停止时，它的当前位置将被存储在 EEPROM 中。控制器关闭后，转盘不会因外力移动而明显移动。
编辑评论+答案
我已经做了一个简单的状态机。
我没有使用任何库来控制步进器，我目前使用 micros() 计时器来设置步进。
宏 REPEAT_MS 和 END_REPEAT 之间的所有代码在 us
中以给定的间隔运行void setSteps()
{
uint8_t speedVar = 255 - speed ;
REPEAT_US( speedVar ) ;
if( enabled )
{
if( !digitalRead( dirPin ) )
{
if( ++position == maxPos+1 ) position = 1 ; // verify this code if positions work well
digitalWrite( stepPin, HIGH ) ;
digitalWrite( stepPin, LOW ) ; // slow enough for mininum pulse duration
}
else
{
//if( --position == 0 ) position = maxPos ; // currently run 1 direction
}
}
Serial.print("position: ");
Serial.println( position ) ;
else
{
digitalWrite( stepPin, state ) ;// may become port manipulated
return ;
}
END_REPEAT
}
控制速度：
void manageSpeed()
{
REPEAT_MS( ACCELERATION_FACTOR ) ; // separate fixed timing for constant acceleration and deceleration
static uint8_t speedPrev = 255 ;
if( speedSetpoint > speed ) speed ++ ;
if( speedSetpoint < speed ) speed -- ;
if( speed == 0 ) enabled = false ; // if speed reaches 0, stop sending pulses
if( speedPrev != speed ) {
speedPrev = speed ;
Serial.print("SPEED :") ;
Serial.println( speed ) ;
}
END_REPEAT
}
和不太重要的使用宏（它们工作正常，经过良好测试）
#define REPEAT_MS(x) { \
static uint32_t previousTime ;\
uint32_t currentTime = millis() ;\
if( currentTime - previousTime >= x ) {\
previousTime = currentTime ;
// code to be repeated goes between these 2 macros
#define REPEAT_US(x) { \
static uint32_t previousTime ;\
uint32_t currentTime = micros() ;\
if( currentTime - previousTime >= x ) {\
previousTime = currentTime ;
// code to be repeated goes between these 2 macros
#define END_REPEAT } \
}
我将仔细研究并尝试实现梯形控制器并尝试实现建议的公式。
第二次编辑： 我目前有一些工作代码，但有一件事我无法理解。
目前我是每100ms调速1。1是加速因子。总加速时间是 3500ms 因为 35 是我的最大速度。
这是加速时的输出。我不得不微调一下步长间隔。 速度：34 上次：100 总时间：3400 距离：6853 总排名：1147
SPEED :35
Last Time: 100 total time: 3500
distanceToGo: 6784
total position: 1216
cruising
这个 1216 应该是 1225。P(t) = 0.5 * 1 * 35^2 = 1225 个脉冲。然而，它已经足够接近了。
比减速：
braking
SPEED :34
Last Time: 27 total time: 11400
distanceToGo: 1207
total position: 6793
SPEED :33
Last Time: 100 total time: 11500
distanceToGo: 1137
total position: 6863
...
SPEED :0
Last Time: 100 total time: 14800
distanceToGo: -8
total position: 8008
Last Time: 1 total time: 14801
distanceToGo: -8
total position: 8008
我有 8 的超调，这是可以接受的。但是我没有得到的是以下内容。现在step时间间隔的计算是
REPEAT_US( 47950 / speed ) ;
47950除了不完美之外，也是无法估量的。我通过反复试验找到了它，无法解释为什么会是这样。
位置控制器方法大致如何工作？
据我了解，您需要的是一个位置控制器，具有从目标位置到目标位置的定义加速度和减速度。最简单的运动控制形式是“梯形”控制器——即具有线性加速度、恒定最大值和线性减速度的控制器；如果您绘制速度随时间变化的曲线，则形成一个梯形。当位置变化较小时，可能达不到最大速度，此时运动呈三角形。
一个合适的 class 来实现这个可能有：
- 定义梯形的参数（加速度、最大速度、减速度），
- 一个位置范围的参数（在你的情况下是0到20000），这样当设置了一个特定的位置位置设定点时，控制器可以设置最短运动的方向，
- 用于定义目标位置的设定点函数，
- 确定位置误差的更新函数（当前位置与所需位置之间的差异，并发出所需的步数以将误差归零）。此函数只需要足够频繁地调用以确保平滑运动，理想情况下它一次发出的步数不超过一步
这样的 class 的单位可能是步数 (steps/s、steps/s2) 或度数，或其他.我建议使用步进 - 它会提供最好的控制和最低的 CPU 负载，并且它是独立于系统的 - 例如，如果你正在驾驶线性执行器或车辆或有额外的传动装置，度数没有多大意义在任何情况下，您都需要进行额外的计算以将其转换为现实世界的系统域。
棘手的部分可能是确定何时停止加速以及何时开始减速。在这个应用程序中，我建议您保持简单，不要让 set-point/target 位置在一个动作完成之前改变。这使得计算更简单，因为当您规划梯形时，速度将始终为零。同样在这个应用中也不需要支持加速度到速度的动态变化。
所以我们需要的基本方程是：
p(t) = v0t + 0.5at2
使得任何特定时间的位置 t - 即 p(t) - 是初始速度加上一半加速度的平方。
对于梯形相位 (1)、(2)、(3)：
^ _________________
| /| |\
Vel | / | | \
| / | | \
| /(1)| (2) |(3)\
------------------------------>
time
等式是：
- (1) p(t) = 0.5a加速度 。 t2
- (2) p(t) = vmax 。 t
- (3) p(t) = vmax 。 t - 0.5a减速。 t2
注意到在每个阶段t被重置，并且p(t)是相对于阶段开始的位置（增量）。
但是对于短距离来说，运动是三角形的，不会达到vmax。那么你必须在停止距离小于或等于剩余距离时开始减速。
这里还有一个有用的方程式：
- (4) v = 在
您可以在阶段 (1) (v = at) 和阶段 (2) (*v = vmax) 来确定“停止距离” (v2 / 2a) 来确定阶段 (3) 的开始。
因此，我建议将更新功能实现为每个阶段的状态机。
下面的实现已经在 PC 上的测试工具 运行ning 中进行了数值测试。它假定使用标准 Arduino Library Stepper class 来执行实际步进，并且 class 引用 Stepper 对象，因此您可以控制多个电机。我还没有（还）在真实硬件上测试过它。我在 PC 上将 millis() 函数和 Stepper class 存根到 运行。
class cTrapezoidStepper
{
public:
cTrapezoidStepper( Stepper& stepper, int32_t accel, int32_t vmax, int32_t decel, int32_t range ) :
m_stepper( stepper ),
m_accel( accel ),
m_vmax( vmax ),
m_vpeak( 0 ),
m_decel( decel ),
m_range( range ),
m_start_time( 0 ),
m_current_pos( 0 ),
m_target_delta( 0 ),
m_direction( 0 ),
m_distance_moved( 0 ),
m_trap_phase( STOP )
{
}
bool isMoving()
{
return m_trap_phase != 0 ;
}
int32_t setTargetPos( int32_t target )
{
if( m_trap_phase == STOP &&
target != m_current_pos )
{
// Determine number of steps and direction to target
m_target_delta = target % m_range - m_current_pos ;
if( abs( m_target_delta ) > m_range / 2 )
{
m_target_delta -= m_range ;
}
m_direction = 1 ;
if( m_target_delta < 0 )
{
m_target_delta = -m_target_delta ;
m_direction = -1 ;
}
// Init motion
m_start_time = tick() ;
m_distance_moved = 0 ;
m_trap_phase = ACCEL ;
}
return m_target_delta ;
}
int32_t update()
{
int32_t t = tick() - m_start_time ;
int32_t distance_to_move = m_distance_moved ;
int32_t distance_to_target = m_target_delta - m_distance_moved ;
int32_t current_velocity = 0 ;
int32_t stopping_distance = 0 ;
switch( m_trap_phase )
{
case ACCEL :
{
current_velocity = (m_accel * t) / ONE_SECOND ;
stopping_distance = (current_velocity * current_velocity) / (2 * m_decel) ;
m_vpeak = current_velocity ;
if( distance_to_target <= stopping_distance )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = DECEL ;
}
else if( current_velocity >= m_vmax )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = CONSTANT ;
}
else
{
distance_to_move = m_accel * (t * t) / (2 * ONE_SECOND * ONE_SECOND) ;
}
}
break ;
case CONSTANT :
{
m_vpeak = m_vmax ;
current_velocity = m_vmax ;
stopping_distance = (current_velocity * current_velocity) / (2 * m_decel) ;
if( distance_to_target <= stopping_distance )
{
// Reset for next phase
m_start_time = tick() ;
m_target_delta = distance_to_target ;
m_distance_moved = 0 ;
distance_to_move = 0 ;
m_trap_phase = DECEL ;
}
else
{
distance_to_move = (t * m_vmax) / ONE_SECOND ;
}
}
break ;
case DECEL :
{
current_velocity = m_vpeak - (m_decel * t) / ONE_SECOND ;
if( distance_to_target <= 0 )
{
m_trap_phase = STOP ;
}
else
{
distance_to_move = (m_vpeak * t) / ONE_SECOND -
(m_decel * (t * t) / (2 * ONE_SECOND * ONE_SECOND)) ;
}
}
break ;
default :
{
distance_to_move = m_distance_moved ;
}
break ;
}
// Do steps
int32_t steps = (distance_to_move - m_distance_moved) * m_direction ;
m_stepper.step( steps ) ;
// Update position
m_distance_moved = distance_to_move ;
m_current_pos += steps ;
m_current_pos %= m_range ;
return m_current_pos ;
}
private :
static const int32_t ONE_SECOND = 100 ;
Stepper& m_stepper ;
int32_t m_accel ;
int32_t m_vmax ;
int32_t m_vpeak ;
int32_t m_decel ;
int32_t m_range ;
unsigned long m_start_time ;
int32_t m_current_pos ;
int32_t m_target_delta ;
int32_t m_direction ;
int32_t m_distance_moved ;
enum
{
STOP,
ACCEL,
CONSTANT,
DECEL
} m_trap_phase ;
int32_t tick(){ return millis() / (1000 / ONE_SECOND) ; }
};
测试（Windows 不是 Arduino 代码，也不是你如何在 Sketch 中使用它 loop()）看起来像这样：
int main()
{
Stepper dummy ;
cTrapezoidStepper turntable( dummy, 100, 600, 150, 20000 ) ;
turntable.setTargetPos( 5000 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 2500 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 6000 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
Sleep(5000) ;
turntable.setTargetPos( 0 ) ;
while( turntable.isMoving() )
{
turntable.update() ;
}
}
所以
cTrapezoidStepper turntable( dummy, 100, 600, 150, 20000 ) ;
实例化一个控制器：
- 加速度 100 steps/s2
- 最大速度600steps/s
- 加速度150steps/s2
- 步长范围 0 到 19999 步。
当您设置位置时，它会选择最短的方向到达目标。
测试依次移动到位置 5000、2500、6000 然后 0，每次移动之间有一个延迟。药水配置文件如下：
另请注意，第二个移动是三角形的，因为距离太短，无法达到最大速度。
要在 Sketch 中执行类似的测试，您可能需要 loop() 如：
void loop()
{
int32_t positions[] = { 19000, 4000, 2500, 6000, 0 } ;
static int test = -1 ;
if( !turntable.isMoving() )
{
test = (test + 1) % (sizeof(positions) / sizeof(*positions)) ;
turntable.setTargetPos( positions[test] ) ;
}
else
{
turntable.update() ;
}
}
请注意我实现的成员函数 cTrapezoidStepper::tick() 以将计时分辨率降低到 100 毫秒间隔。这对于平滑控制来说应该没问题，但对于防止算术溢出是必要的（如果你使用非常低的加速度你可能仍然会遇到 - 如果你想让它更通用，那部分可以改进）。