AVR446步进电机加减速算法移植

最近产品主控换为国产芯片需要移植程序，正好用到步进电机加减速算法，在这里记录一下走过的坑

一，遇到的问题

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();
}