605 lines
16 KiB
C
605 lines
16 KiB
C
#include "stepper_control.h"
|
|
#include "timer_driver.h"
|
|
#include "sys_data_bus.h"
|
|
#include "limit_switch.h"
|
|
#include "system_driver.h"
|
|
#include "config.h"
|
|
#include <stddef.h>
|
|
|
|
#define STEPPER_UPDATE_INTERVAL 10
|
|
#define STEPPER_CALIB_SPEED 80
|
|
#define STEPPER_TOTAL_STEPS_DEFAULT 2000
|
|
#define STEPPER_CALIB_TIMEOUT_MS 10000
|
|
|
|
#define ANGLE_TABLE_SIZE 64
|
|
#define ANGLE_RESOLUTION 1024
|
|
#define ANGLE_PER_STEP (ANGLE_RESOLUTION / 4)
|
|
|
|
static const uint16_t sine_table[ANGLE_TABLE_SIZE] = {
|
|
1800, 1976, 2151, 2323, 2489, 2649, 2800, 2942,
|
|
3073, 3191, 3297, 3387, 3463, 3522, 3565, 3591,
|
|
3600, 3591, 3565, 3522, 3463, 3387, 3297, 3191,
|
|
3073, 2942, 2800, 2649, 2489, 2323, 2151, 1976,
|
|
1800, 1624, 1449, 1277, 1111, 951, 800, 658,
|
|
527, 409, 303, 213, 137, 78, 35, 9,
|
|
0, 9, 35, 78, 137, 213, 303, 409,
|
|
527, 658, 800, 951, 1111, 1277, 1449, 1624,
|
|
};
|
|
|
|
static volatile int32_t current_position;
|
|
static int32_t target_position;
|
|
static int32_t start_position;
|
|
static float current_speed;
|
|
static float accel_distance;
|
|
static float decel_distance;
|
|
static float total_distance;
|
|
static uint8_t running;
|
|
static uint8_t calibrated;
|
|
static uint8_t di_max_installed;
|
|
static int32_t calibrated_steps;
|
|
static int32_t origin_steps;
|
|
static int32_t max_steps;
|
|
static stepper_state_t state;
|
|
static uint32_t last_update_tick;
|
|
static uint32_t calib_start_tick;
|
|
static uint8_t calib_found_min;
|
|
static uint8_t calib_found_max;
|
|
static int32_t calib_min_position;
|
|
static int32_t calib_max_position;
|
|
static uint16_t last_hold_duty_a;
|
|
static uint16_t last_hold_duty_b;
|
|
|
|
static float s_curve_factor(float progress)
|
|
{
|
|
if (progress <= 0.0f) return 0.0f;
|
|
if (progress >= 1.0f) return 1.0f;
|
|
return progress * progress * (3.0f - 2.0f * progress);
|
|
}
|
|
|
|
static uint16_t stepper_lookup_sine(uint16_t index, uint8_t frac)
|
|
{
|
|
uint16_t s0 = sine_table[index];
|
|
uint16_t s1 = sine_table[(index + 1) & (ANGLE_TABLE_SIZE - 1)];
|
|
return s0 + (((s1 - s0) * frac) >> 4);
|
|
}
|
|
|
|
static void stepper_update_phases(void)
|
|
{
|
|
uint16_t angle = (uint16_t)((current_position * ANGLE_PER_STEP) & (ANGLE_RESOLUTION - 1));
|
|
uint8_t idx_a = angle >> 4;
|
|
uint8_t frac = (uint8_t)(angle & 0x0F);
|
|
uint8_t idx_b = (idx_a + 16) & (ANGLE_TABLE_SIZE - 1);
|
|
|
|
uint16_t duty_a = stepper_lookup_sine(idx_a, frac);
|
|
uint16_t duty_b = stepper_lookup_sine(idx_b, frac);
|
|
|
|
timer_pwm_set_duty(TIMER_CHANNEL_1, duty_a);
|
|
timer_pwm_set_duty(TIMER_CHANNEL_2, duty_b);
|
|
|
|
last_hold_duty_a = duty_a;
|
|
last_hold_duty_b = duty_b;
|
|
}
|
|
|
|
static void stepper_set_hold(void)
|
|
{
|
|
uint16_t hold_a = (uint16_t)(((uint32_t)last_hold_duty_a * STEPPER_HOLD_CURRENT) / 100);
|
|
uint16_t hold_b = (uint16_t)(((uint32_t)last_hold_duty_b * STEPPER_HOLD_CURRENT) / 100);
|
|
timer_pwm_set_duty(TIMER_CHANNEL_1, hold_a);
|
|
timer_pwm_set_duty(TIMER_CHANNEL_2, hold_b);
|
|
}
|
|
|
|
static void stepper_phases_off(void)
|
|
{
|
|
timer_pwm_set_duty(TIMER_CHANNEL_1, STEPPER_PWM_CENTER);
|
|
timer_pwm_set_duty(TIMER_CHANNEL_2, STEPPER_PWM_CENTER);
|
|
}
|
|
|
|
static void set_speed_to_data_bus(float speed)
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.f = speed;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_SPEED, val);
|
|
}
|
|
|
|
static void set_position_to_data_bus(int32_t pos)
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.i = pos;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);
|
|
}
|
|
|
|
static void set_state_to_data_bus(stepper_state_t s)
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.u = (uint32_t)s;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATE, val);
|
|
}
|
|
|
|
static void stepper_apply_limit(void)
|
|
{
|
|
if (limit_switch_get_state(LIMIT_SW_MIN) == LIMIT_SW_STATE_TRIGGERED) {
|
|
if (current_position < origin_steps) {
|
|
current_position = origin_steps;
|
|
set_position_to_data_bus(current_position);
|
|
}
|
|
}
|
|
|
|
if (limit_switch_get_state(LIMIT_SW_MAX) == LIMIT_SW_STATE_TRIGGERED) {
|
|
if (current_position > max_steps) {
|
|
current_position = max_steps;
|
|
set_position_to_data_bus(current_position);
|
|
}
|
|
}
|
|
}
|
|
|
|
ret_code_t stepper_init(void)
|
|
{
|
|
current_position = 0;
|
|
target_position = 0;
|
|
start_position = 0;
|
|
current_speed = 0.0f;
|
|
running = 0;
|
|
calibrated = 0;
|
|
state = STEPPER_STATE_IDLE;
|
|
last_update_tick = system_get_tick();
|
|
last_hold_duty_a = STEPPER_PWM_CENTER;
|
|
last_hold_duty_b = STEPPER_PWM_CENTER;
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
|
|
val.u = 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_CURRENT, val);
|
|
|
|
val.i = 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_TARGET, val);
|
|
|
|
val.f = 0.0f;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_SPEED, val);
|
|
|
|
val.u = STEPPER_STATE_IDLE;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATE, val);
|
|
|
|
val.i = STEPPER_TOTAL_STEPS_DEFAULT;
|
|
sys_data_bus_write(SYS_DATA_BUS_MAX_STEPS, val);
|
|
|
|
val.i = 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_ORIGIN_STEPS, val);
|
|
sys_data_bus_write(SYS_DATA_BUS_CALIBRATED_STEPS, val);
|
|
|
|
val.u = 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_IS_CALIBRATED, val);
|
|
sys_data_bus_write(SYS_DATA_BUS_IS_DI_MAX_INSTALLED, val);
|
|
}
|
|
|
|
origin_steps = 0;
|
|
max_steps = STEPPER_TOTAL_STEPS_DEFAULT;
|
|
calibrated_steps = STEPPER_TOTAL_STEPS_DEFAULT;
|
|
di_max_installed = 1;
|
|
|
|
timer_stepper_pwm_init(20000, STEPPER_PWM_RESOLUTION);
|
|
|
|
return RET_OK;
|
|
}
|
|
|
|
ret_code_t stepper_deinit(void)
|
|
{
|
|
running = 0;
|
|
state = STEPPER_STATE_IDLE;
|
|
stepper_phases_off();
|
|
timer_pwm_stop();
|
|
return RET_OK;
|
|
}
|
|
|
|
ret_code_t stepper_move_to(int32_t target)
|
|
{
|
|
sys_data_bus_value_t val;
|
|
|
|
if (state == STEPPER_STATE_CALIBRATING) {
|
|
return RET_BUSY;
|
|
}
|
|
|
|
if (target < origin_steps) {
|
|
target = origin_steps;
|
|
}
|
|
if (target > max_steps) {
|
|
target = max_steps;
|
|
}
|
|
|
|
target_position = target;
|
|
|
|
val.i = target;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_TARGET, val);
|
|
|
|
start_position = current_position;
|
|
total_distance = (float)(target_position - current_position);
|
|
|
|
if (total_distance == 0.0f) {
|
|
stepper_set_hold();
|
|
return RET_OK;
|
|
}
|
|
|
|
current_speed = (float)STEPPER_SPEED_START;
|
|
running = 1;
|
|
state = STEPPER_STATE_ACCEL;
|
|
|
|
set_state_to_data_bus(state);
|
|
|
|
{
|
|
float total_abs = total_distance;
|
|
if (total_abs < 0.0f) total_abs = -total_abs;
|
|
|
|
accel_distance = (float)STEPPER_ACCEL_STEPS;
|
|
decel_distance = (float)STEPPER_DECEL_STEPS;
|
|
|
|
if (accel_distance + decel_distance > total_abs) {
|
|
float ratio = total_abs / (accel_distance + decel_distance);
|
|
accel_distance *= ratio;
|
|
decel_distance *= ratio;
|
|
}
|
|
}
|
|
|
|
stepper_update_phases();
|
|
|
|
return RET_OK;
|
|
}
|
|
|
|
ret_code_t stepper_move_rel(int32_t steps)
|
|
{
|
|
return stepper_move_to(current_position + steps);
|
|
}
|
|
|
|
ret_code_t stepper_stop(void)
|
|
{
|
|
if (running) {
|
|
state = STEPPER_STATE_DECEL;
|
|
set_state_to_data_bus(state);
|
|
}
|
|
return RET_OK;
|
|
}
|
|
|
|
ret_code_t stepper_emergency_stop(void)
|
|
{
|
|
running = 0;
|
|
current_speed = 0.0f;
|
|
state = STEPPER_STATE_STOPPED;
|
|
stepper_set_hold();
|
|
set_speed_to_data_bus(0.0f);
|
|
set_state_to_data_bus(state);
|
|
return RET_OK;
|
|
}
|
|
|
|
ret_code_t stepper_calibrate(void)
|
|
{
|
|
if (state == STEPPER_STATE_CALIBRATING) {
|
|
return RET_BUSY;
|
|
}
|
|
|
|
calib_found_min = 0;
|
|
calib_found_max = 0;
|
|
calib_min_position = 0;
|
|
calib_max_position = 0;
|
|
calib_start_tick = system_get_tick();
|
|
|
|
current_position = 0;
|
|
state = STEPPER_STATE_CALIBRATING;
|
|
running = 1;
|
|
|
|
stepper_update_phases();
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.u = 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_IS_CALIBRATED, val);
|
|
}
|
|
|
|
return RET_OK;
|
|
}
|
|
|
|
static void stepper_run_calibrate(void)
|
|
{
|
|
uint32_t now = system_get_tick();
|
|
|
|
if ((now - calib_start_tick) > STEPPER_CALIB_TIMEOUT_MS) {
|
|
state = STEPPER_STATE_STOPPED;
|
|
running = 0;
|
|
stepper_set_hold();
|
|
return;
|
|
}
|
|
|
|
if (!calib_found_min) {
|
|
if (limit_switch_get_state(LIMIT_SW_MIN) == LIMIT_SW_STATE_TRIGGERED) {
|
|
calib_found_min = 1;
|
|
calib_min_position = current_position;
|
|
origin_steps = current_position;
|
|
current_position = 0;
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.i = current_position;
|
|
sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);
|
|
sys_data_bus_write(SYS_DATA_BUS_ORIGIN_STEPS, val);
|
|
}
|
|
} else {
|
|
current_position -= 1;
|
|
set_position_to_data_bus(current_position);
|
|
stepper_update_phases();
|
|
system_delay_ms(5);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!calib_found_max) {
|
|
{
|
|
sys_data_bus_value_t di_max_val;
|
|
di_max_val = sys_data_bus_read(SYS_DATA_BUS_DI_MAX_STATE);
|
|
if (di_max_val.u != 0) {
|
|
di_max_installed = 1;
|
|
}
|
|
}
|
|
|
|
if (di_max_installed && limit_switch_get_state(LIMIT_SW_MAX) == LIMIT_SW_STATE_TRIGGERED) {
|
|
calib_found_max = 1;
|
|
calib_max_position = current_position;
|
|
max_steps = current_position;
|
|
calibrated_steps = max_steps - origin_steps;
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.i = max_steps;
|
|
sys_data_bus_write(SYS_DATA_BUS_MAX_STEPS, val);
|
|
}
|
|
} else if (!di_max_installed) {
|
|
if (current_position >= calibrated_steps) {
|
|
calib_found_max = 1;
|
|
max_steps = calibrated_steps;
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
val.i = max_steps;
|
|
sys_data_bus_write(SYS_DATA_BUS_MAX_STEPS, val);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!calib_found_max) {
|
|
current_position += 1;
|
|
set_position_to_data_bus(current_position);
|
|
stepper_update_phases();
|
|
system_delay_ms(5);
|
|
}
|
|
return;
|
|
}
|
|
|
|
{
|
|
sys_data_bus_value_t val;
|
|
calibrated = 1;
|
|
val.u = 1;
|
|
sys_data_bus_write(SYS_DATA_BUS_IS_CALIBRATED, val);
|
|
|
|
val.i = calibrated_steps;
|
|
sys_data_bus_write(SYS_DATA_BUS_CALIBRATED_STEPS, val);
|
|
|
|
val.u = di_max_installed ? 1 : 0;
|
|
sys_data_bus_write(SYS_DATA_BUS_IS_DI_MAX_INSTALLED, val);
|
|
}
|
|
|
|
stepper_phases_off();
|
|
running = 0;
|
|
state = STEPPER_STATE_IDLE;
|
|
set_state_to_data_bus(state);
|
|
}
|
|
|
|
static void stepper_run_move(void)
|
|
{
|
|
uint32_t now;
|
|
uint32_t dt_ms;
|
|
float dt;
|
|
float abs_remaining;
|
|
float abs_traveled;
|
|
float speed_range;
|
|
|
|
now = system_get_tick();
|
|
dt_ms = now - last_update_tick;
|
|
|
|
if (dt_ms < STEPPER_UPDATE_INTERVAL) {
|
|
return;
|
|
}
|
|
|
|
if (dt_ms > 50) {
|
|
dt_ms = STEPPER_UPDATE_INTERVAL;
|
|
}
|
|
|
|
last_update_tick = now;
|
|
dt = (float)dt_ms / 1000.0f;
|
|
|
|
abs_traveled = (float)(current_position - start_position);
|
|
if (abs_traveled < 0.0f) abs_traveled = -abs_traveled;
|
|
|
|
abs_remaining = total_distance - (float)(current_position - start_position);
|
|
if (abs_remaining < 0.0f) abs_remaining = 0.0f;
|
|
|
|
stepper_apply_limit();
|
|
|
|
if (limit_switch_get_state(LIMIT_SW_MIN) == LIMIT_SW_STATE_TRIGGERED ||
|
|
limit_switch_get_state(LIMIT_SW_MAX) == LIMIT_SW_STATE_TRIGGERED) {
|
|
if (state != STEPPER_STATE_DECEL) {
|
|
state = STEPPER_STATE_DECEL;
|
|
set_state_to_data_bus(state);
|
|
}
|
|
}
|
|
|
|
speed_range = (float)(STEPPER_SPEED_MAX - STEPPER_SPEED_START);
|
|
|
|
switch (state) {
|
|
case STEPPER_STATE_ACCEL: {
|
|
float accel_end = accel_distance;
|
|
|
|
if (abs_traveled >= accel_end) {
|
|
state = STEPPER_STATE_CONSTANT;
|
|
current_speed = (float)STEPPER_SPEED_MAX;
|
|
set_state_to_data_bus(state);
|
|
} else {
|
|
float progress = abs_traveled / accel_end;
|
|
float sf = s_curve_factor(progress);
|
|
current_speed = (float)STEPPER_SPEED_START + speed_range * sf;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case STEPPER_STATE_CONSTANT: {
|
|
float decel_start = total_distance - decel_distance;
|
|
|
|
if (total_distance > 0.0f && abs_traveled >= decel_start) {
|
|
state = STEPPER_STATE_DECEL;
|
|
set_state_to_data_bus(state);
|
|
} else if (total_distance < 0.0f && abs_traveled >= -decel_start) {
|
|
state = STEPPER_STATE_DECEL;
|
|
set_state_to_data_bus(state);
|
|
} else {
|
|
current_speed = (float)STEPPER_SPEED_MAX;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case STEPPER_STATE_DECEL: {
|
|
float decel_remaining;
|
|
|
|
if (total_distance > 0.0f) {
|
|
decel_remaining = total_distance - abs_traveled;
|
|
} else {
|
|
decel_remaining = -total_distance - abs_traveled;
|
|
}
|
|
|
|
if (decel_remaining <= 0.5f) {
|
|
current_position = target_position;
|
|
current_speed = 0.0f;
|
|
state = STEPPER_STATE_STOPPED;
|
|
running = 0;
|
|
set_position_to_data_bus(current_position);
|
|
set_speed_to_data_bus(0.0f);
|
|
stepper_update_phases();
|
|
stepper_set_hold();
|
|
set_state_to_data_bus(state);
|
|
return;
|
|
}
|
|
|
|
{
|
|
float decel_prog = 1.0f - (decel_remaining / decel_distance);
|
|
if (decel_prog < 0.0f) decel_prog = 0.0f;
|
|
if (decel_prog > 1.0f) decel_prog = 1.0f;
|
|
|
|
float sf = s_curve_factor(decel_prog);
|
|
current_speed = (float)STEPPER_SPEED_MAX - speed_range * sf;
|
|
if (current_speed < (float)STEPPER_SPEED_START) {
|
|
current_speed = (float)STEPPER_SPEED_START;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
default:
|
|
return;
|
|
}
|
|
|
|
{
|
|
float speed_step = current_speed * dt;
|
|
|
|
if (total_distance > 0.0f) {
|
|
current_position += (int32_t)speed_step;
|
|
if (current_position > target_position) {
|
|
current_position = target_position;
|
|
}
|
|
} else {
|
|
current_position -= (int32_t)speed_step;
|
|
if (current_position < target_position) {
|
|
current_position = target_position;
|
|
}
|
|
}
|
|
|
|
if (current_position > max_steps) {
|
|
current_position = max_steps;
|
|
}
|
|
if (current_position < origin_steps) {
|
|
current_position = origin_steps;
|
|
}
|
|
}
|
|
|
|
stepper_update_phases();
|
|
|
|
set_position_to_data_bus(current_position);
|
|
set_speed_to_data_bus(current_speed);
|
|
}
|
|
|
|
void stepper_run(void)
|
|
{
|
|
if (!running) {
|
|
return;
|
|
}
|
|
|
|
if (state == STEPPER_STATE_CALIBRATING) {
|
|
stepper_run_calibrate();
|
|
return;
|
|
}
|
|
|
|
stepper_run_move();
|
|
}
|
|
|
|
int32_t stepper_get_position(void)
|
|
{
|
|
return current_position;
|
|
}
|
|
|
|
int32_t stepper_get_target(void)
|
|
{
|
|
return target_position;
|
|
}
|
|
|
|
stepper_state_t stepper_get_state(void)
|
|
{
|
|
return state;
|
|
}
|
|
|
|
ret_code_t stepper_move_to_min(void)
|
|
{
|
|
return stepper_move_to(origin_steps);
|
|
}
|
|
|
|
ret_code_t stepper_move_to_max(void)
|
|
{
|
|
return stepper_move_to(max_steps);
|
|
}
|
|
|
|
ret_code_t stepper_move_to_angle(float angle_deg)
|
|
{
|
|
int32_t steps;
|
|
|
|
if (angle_deg < 0.0f) angle_deg = 0.0f;
|
|
if (angle_deg > 360.0f) angle_deg = 360.0f;
|
|
|
|
steps = (int32_t)((calibrated_steps * angle_deg) / 360.0f);
|
|
|
|
return stepper_move_to(origin_steps + steps);
|
|
}
|
|
|
|
uint8_t stepper_is_running(void)
|
|
{
|
|
return running;
|
|
}
|
|
|
|
int32_t stepper_get_max_steps(void)
|
|
{
|
|
return max_steps;
|
|
}
|
|
|
|
int32_t stepper_get_origin_steps(void)
|
|
{
|
|
return origin_steps;
|
|
}
|
|
|
|
uint8_t stepper_is_calibrated(void)
|
|
{
|
|
return calibrated;
|
|
}
|