最近产品主控换为国产芯片需要移植程序,正好用到步进电机加减速算法,在这里记录一下走过的坑
一,遇到的问题
1,无法匀速运行,加速完成后直接进去减速,也是下面这两个数据类型不匹配,计算出错了,需要将两个数据类型改为一样的,uint16_t
// Holds next delay period.
uint16_t new_step_delay;
// Remember the last step delay used when accelrating.
static uint16_t last_accel_delay;
2,减速状态下无法减速,计算余数的变量数据类型不对,会导致计算出错,需要将变量数据类型改为,uint16_t
// Keep track of remainder from new_step-delay calculation to incrase accurancy
static unsigned int rest = 0;
下面看移植源码:
先是头文件:
#ifndef SPEED_CNTR_H
#define SPEED_CNTR_H
#include "driver_timer.h"
#include "co_printf.h"
typedef struct
{
//! What part of the speed ramp we are in.
unsigned char run_state : 3;
//! Direction stepper motor should move.
unsigned char dir : 1;
//! Peroid of next timer delay. At start this value set the accelration rate.
unsigned int step_delay;
//! What step_pos to start decelaration
unsigned int decel_start;
//! Sets deceleration rate.
signed int decel_val;
//! Minimum time delay (max speed)
signed int min_delay;
//! Counter used when accelerateing/decelerateing to calculate step_delay.
signed int accel_count;
} speedRampData;
/*! \Brief Frequency of timer1 in [Hz].
*
* Modify this according to frequency used. Because of the prescaler setting,
* the timer1 frequency is the clock frequency divided by 8.
*/
// Timer/Counter 1 running on 3,686MHz / 8 = 460,75kHz (2,17uS). (T1-FREQ 460750)
#define T1_FREQ 3000000
//! Number of (full)steps per round on stepper motor in use.
#define FSPR 200
#define SPR (FSPR * 8)
// Maths constants. To simplify maths when calculating in speed_cntr_Move().
#define ALPHA (2 * 3.14159 / SPR) // 2*pi/spr
#define A_T_x100 ((long)(ALPHA * T1_FREQ * 100)) // (ALPHA / T1_FREQ)*100
#define T1_FREQ_148 ((int)((T1_FREQ * 0.676) / 100)) // divided by 100 and scaled by 0.676
#define A_SQ (long)(ALPHA * 2 * 10000000000) // ALPHA*2*10000000000
#define A_x20000 (int)(ALPHA * 20000) // ALPHA*20000
// Speed ramp states
#define STOP 0
#define ACCEL 1
#define DECEL 2
#define RUN 3
void speed_cntr_Move(signed int step, unsigned int accel, unsigned int decel, unsigned int speed);
void speed_cntr_Init(void);
static unsigned long sqrt(unsigned long v);
unsigned int min(unsigned int x, unsigned int y);
#endif
#include "sm_driver.h"
#include "speed_cntr.h"
speedRampData srd;
void speed_cntr_Move(signed int step, unsigned int accel, unsigned int decel, unsigned int speed)
{
//! Number of steps before we hit max speed.
unsigned int max_s_lim;
//! Number of steps before we must start deceleration (if accel does not hit max speed).
unsigned int accel_lim;
// Set direction from sign on step value.
if (step < 0)
{
srd.dir = CCW;
step = -step;
}
else
{
srd.dir = CW;
}
// If moving only 1 step.
if (step == 1)
{
// Move one step...
srd.accel_count = -1;
// ...in DECEL state.
srd.run_state = DECEL;
// Just a short delay so main() can act on 'running'.
srd.step_delay = 1000;
motor_init(TIMER0, 1000, TIMER_PERIODIC);
timer_run(TIMER0);
}
// Only move if number of steps to move is not zero.
else if (step != 0)
{
// Refer to documentation for detailed information about these calculations.
// Set max speed limit, by calc min_delay to use in timer.
// min_delay = (alpha / tt)/ w
srd.min_delay = A_T_x100 / speed;
// Set accelration by calc the first (c0) step delay .
// step_delay = 1/tt * sqrt(2*alpha/accel)
// step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000
srd.step_delay = (T1_FREQ_148 * sqrt(A_SQ / accel)) / 100;
// Find out after how many steps does the speed hit the max speed limit.
// max_s_lim = speed^2 / (2*alpha*accel)
max_s_lim = (long)speed * speed / (long)(((long)A_x20000 * accel) / 100);
// If we hit max speed limit before 0,5 step it will round to 0.
// But in practice we need to move atleast 1 step to get any speed at all.
if (max_s_lim == 0)
{
max_s_lim = 1;
}
// Find out after how many steps we must start deceleration.
// n1 = (n1+n2)decel / (accel + decel)
accel_lim = ((long)step * decel) / (accel + decel);
// We must accelrate at least 1 step before we can start deceleration.
if (accel_lim == 0)
{
accel_lim = 1;
}
// Use the limit we hit first to calc decel.
if (accel_lim <= max_s_lim)
{
srd.decel_val = accel_lim - step;
}
else
{
srd.decel_val = -((long)max_s_lim * accel) / decel;
// srd.decel_val = -(int16_t)(max_s_lim * accel / decel);
}
// We must decelrate at least 1 step to stop.
if (srd.decel_val == 0)
{
srd.decel_val = -1;
}
// Find step to start decleration.
srd.decel_start = step + srd.decel_val;
// If the maximum speed is so low that we dont need to go via accelration state.
if (srd.step_delay <= srd.min_delay)
{
srd.step_delay = srd.min_delay;
srd.run_state = RUN;
}
else
{
srd.run_state = ACCEL;
}
// Reset counter.
srd.accel_count = 0;
motor_init(TIMER0, 1000, TIMER_PERIODIC);
timer_run(TIMER0);
}
}
void speed_cntr_Init(void)
{
int steps = 1600;
// Accelration to use.
int acceleration = 100;
// Deceleration to use.
int deceleration = 100;
// Speed to use.
int speed = 500;
// Tells what part of speed ramp we are in.
srd.run_state = STOP;
motor_init(TIMER0, 1000, TIMER_PERIODIC);
NVIC_SetPriority(TIMER0_IRQn, 2);
NVIC_EnableIRQ(TIMER0_IRQn);
speed_cntr_Move(steps, acceleration, deceleration, speed);
}
void speed_cntr_update(void)
{
// Holds next delay period.
uint16_t new_step_delay;
// Remember the last step delay used when accelrating.
static uint16_t last_accel_delay;
// Counting steps when moving.
static uint32_t step_count = 0;
// Keep track of remainder from new_step-delay calculation to incrase accurancy
static uint16_t rest = 0;
motor_init(TIMER0, srd.step_delay, TIMER_PERIODIC);
timer_run(TIMER0);
switch (srd.run_state)
{
case STOP:
step_count = 0;
rest = 0;
// Stop Timer/Counter 1.
timer_stop(TIMER0);
break;
case ACCEL:
sm_driver_StepCounter(srd.dir);
step_count++;
srd.accel_count++;
new_step_delay = srd.step_delay - (((2 * (long)srd.step_delay) + rest) / (4 * srd.accel_count + 1));
rest = ((2 * (long)srd.step_delay) + rest) % (4 * srd.accel_count + 1);
// Chech if we should start decelration.
if (step_count >= srd.decel_start)
{
srd.accel_count = srd.decel_val;
srd.run_state = DECEL;
}
// Chech if we hitted max speed.
else if (new_step_delay <= srd.min_delay)
{
last_accel_delay = new_step_delay;
new_step_delay = srd.min_delay;
rest = 0;
srd.run_state = RUN;
}
break;
case RUN:
sm_driver_StepCounter(srd.dir);
step_count++;
new_step_delay = srd.min_delay;
// Chech if we should start decelration.
if (step_count >= srd.decel_start)
{
srd.accel_count = srd.decel_val;
// Start decelration with same delay as accel ended with.
new_step_delay = last_accel_delay;
srd.run_state = DECEL;
}
break;
case DECEL:
sm_driver_StepCounter(srd.dir);
step_count++;
srd.accel_count++;
new_step_delay = srd.step_delay - (((2 * (long)srd.step_delay) + rest) / (4 * srd.accel_count + 1));
rest = ((2 * (long)srd.step_delay) + rest) % (4 * srd.accel_count + 1);
// Check if we at last step
if (srd.accel_count >= 0)
{
srd.run_state = STOP;
}
break;
}
srd.step_delay = new_step_delay;
co_printf("run_state = %d step_delay = %d decel_start = %d step_count = %d min_delay = %d accel_count = %d step_count = %d rest = %d\r\n", srd.run_state, srd.step_delay, srd.decel_start, srd.decel_val, srd.min_delay, srd.accel_count, step_count,rest);
}
static unsigned long sqrt(unsigned long x)
{
register unsigned long xr; // result register
register unsigned long q2; // scan-bit register
register unsigned char f; // flag (one bit)
xr = 0; // clear result
q2 = 0x40000000L; // higest possible result bit
do
{
if ((xr + q2) <= x)
{
x -= xr + q2;
f = 1; // set flag
}
else
{
f = 0; // clear flag
}
xr >>= 1;
if (f)
{
xr += q2; // test flag
}
} while (q2 >>= 2); // shift twice
if (xr < x)
{
return xr + 1; // add for rounding
}
else
{
return xr;
}
}
/*! \brief Find minimum value.
*
* Returns the smallest value.
*
* \return Min(x,y).
*/
unsigned int min(unsigned int x, unsigned int y)
{
if (x < y)
{
return x;
}
else
{
return y;
}
}
__attribute__((section("ram_code"))) void timer0_isr_ram(void)
{
timer_clear_interrupt(TIMER0);
speed_cntr_update();
}