initial commit
This commit is contained in:
261
app/by_motion.c
Normal file
261
app/by_motion.c
Normal file
@@ -0,0 +1,261 @@
|
||||
#include "by_motion.h"
|
||||
|
||||
#include "pid.h"
|
||||
#include "by_utils.h"
|
||||
#include "by_debug.h"
|
||||
|
||||
#define DRV_ENABLE() gpio_bits_set(GPIOA, GPIO_PINS_12)
|
||||
#define DRV_DISABLE() gpio_bits_reset(GPIOA, GPIO_PINS_12)
|
||||
|
||||
#define MU (2.854f) // speed = \mu * speed_in_pulse
|
||||
|
||||
by_motor_param param_m1;
|
||||
by_motor_param param_m2;
|
||||
PID_TypeDef pid_m1;
|
||||
PID_TypeDef pid_m2;
|
||||
uint8_t motion_enable_flag;
|
||||
uint8_t motion_busy_flag;
|
||||
uint8_t motion_reset_flag;
|
||||
int16_t time_via;
|
||||
float position_abs;
|
||||
|
||||
void by_motion_set_pwm_m1(int32_t pwm_duty)
|
||||
{
|
||||
pwm_duty = clip_s32(pwm_duty, -900, 900); // 不可以拉满哦
|
||||
|
||||
if (pwm_duty < 0) {
|
||||
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, -pwm_duty);
|
||||
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, 0);
|
||||
} else {
|
||||
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, 0);
|
||||
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, pwm_duty);
|
||||
}
|
||||
}
|
||||
|
||||
// void by_motion_set_pwm_m2(int32_t pwm_duty)
|
||||
// {
|
||||
// pwm_duty = clip_s32(pwm_duty, -900, 900); // 不可以拉满哦
|
||||
|
||||
// if (pwm_duty < 0) {
|
||||
// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, -pwm_duty);
|
||||
// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, 0);
|
||||
// } else {
|
||||
// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_3, 0);
|
||||
// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_4, pwm_duty);
|
||||
// }
|
||||
// }
|
||||
|
||||
int16_t by_motion_get_speed_m1(void)
|
||||
{
|
||||
#define alpha (0.1f)
|
||||
static float last_speed = 0.0f;
|
||||
param_m1.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(+1 * (int16_t)tmr_counter_value_get(TMR3));
|
||||
last_speed = param_m1.real_speed;
|
||||
tmr_counter_value_set(TMR3, 0);
|
||||
return (int16_t)param_m1.real_speed;
|
||||
#undef alpha
|
||||
}
|
||||
|
||||
// int16_t by_motion_get_speed_m2(void)
|
||||
// {
|
||||
// #define alpha (0.1f)
|
||||
// static float last_speed = 0.0f;
|
||||
// param_m2.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
|
||||
// last_speed = param_m2.real_speed;
|
||||
// tmr_counter_value_set(TMR3, 0);
|
||||
// return (int16_t)param_m2.real_speed;
|
||||
// #undef alpha
|
||||
// }
|
||||
|
||||
void by_motion_set_speed_m1(int16_t speed)
|
||||
{
|
||||
param_m1.target_speed = speed;
|
||||
}
|
||||
|
||||
// void by_motion_set_speed_m2(int16_t speed)
|
||||
// {
|
||||
// param_m2.target_speed = speed;
|
||||
// }
|
||||
|
||||
void by_motion_set_distance(float distance, int16_t speed)
|
||||
{
|
||||
// TODO 验证距离是否超限
|
||||
if (motion_busy_flag) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (speed < 0) {
|
||||
speed = -speed;
|
||||
}
|
||||
|
||||
position_abs += distance;
|
||||
|
||||
if (distance < 0.0f) {
|
||||
by_motion_set_speed_m1(-1 * speed);
|
||||
distance = -distance;
|
||||
} else {
|
||||
by_motion_set_speed_m1(speed);
|
||||
}
|
||||
|
||||
motion_busy_flag = 1;
|
||||
time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
|
||||
// lwprintf("time_via = %d\r\n", time_via);
|
||||
LOGD("position_abs = %f\r\n", position_abs);
|
||||
}
|
||||
|
||||
void by_motion_set_distance2(float distance, int16_t speed)
|
||||
{
|
||||
// TODO 验证距离是否超限
|
||||
if (motion_busy_flag) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (speed < 0) {
|
||||
speed = -speed;
|
||||
}
|
||||
|
||||
if (distance < 0.0f) {
|
||||
distance = -distance;
|
||||
}
|
||||
|
||||
distance = distance - position_abs;
|
||||
position_abs += distance;
|
||||
|
||||
if (5.0 >= position_abs) {
|
||||
position_abs = 0;
|
||||
by_motion_set_speed_m1(-10);
|
||||
motion_busy_flag = 1;
|
||||
motion_reset_flag = 1;
|
||||
LOGD("RESET position_abs = %f\r\n", position_abs);
|
||||
return;
|
||||
}
|
||||
|
||||
if (distance < 0.0f) {
|
||||
distance = -distance;
|
||||
by_motion_set_speed_m1(-1 * speed);
|
||||
} else {
|
||||
by_motion_set_speed_m1(speed);
|
||||
}
|
||||
|
||||
motion_busy_flag = 1;
|
||||
time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
|
||||
LOGD("position_abs = %f\r\n", position_abs);
|
||||
// lwprintf("time_via = %d\r\n", time_via);
|
||||
}
|
||||
|
||||
void by_motion_init(void)
|
||||
{
|
||||
motion_enable_flag = 0;
|
||||
motion_busy_flag = 0;
|
||||
time_via = 0;
|
||||
position_abs = 0.0f; // TODO 待添加复位操作
|
||||
|
||||
by_motion_set_pwm_m1(0);
|
||||
// by_motion_set_pwm_m2(0);
|
||||
|
||||
/* set default parameters */
|
||||
param_m1.Kp = BY_MOTION_DEFAULT_KP_M1;
|
||||
param_m1.Ki = BY_MOTION_DEFAULT_KI_M1;
|
||||
param_m1.Kd = BY_MOTION_DEFAULT_KD_M1;
|
||||
// param_m2.Kp = BY_MOTION_DEFAULT_KP_M2;
|
||||
// param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
|
||||
// param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
|
||||
|
||||
/* load parameters form ee */
|
||||
// TODO
|
||||
|
||||
PID(&pid_m1, ¶m_m1.real_speed, ¶m_m1.out_pwm, ¶m_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
|
||||
PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC);
|
||||
PID_SetSampleTime(&pid_m1, 20);
|
||||
PID_SetOutputLimits(&pid_m1, -900, 900);
|
||||
|
||||
// PID(&pid_m2, ¶m_m2.real_speed, ¶m_m2.out_pwm, ¶m_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
|
||||
// PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
|
||||
// PID_SetSampleTime(&pid_m2, 10);
|
||||
// PID_SetOutputLimits(&pid_m2, -400, 400);
|
||||
|
||||
motion_enable_flag = 1;
|
||||
DRV_ENABLE();
|
||||
|
||||
// 上电回零
|
||||
by_motion_set_speed_m1(-20);
|
||||
while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_4)) {
|
||||
lwprintf("%f, %f, %f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
|
||||
// 等待复位
|
||||
}
|
||||
by_motion_set_speed_m1(0);
|
||||
by_motion_set_distance2(100, 20);
|
||||
}
|
||||
|
||||
void by_motion_run(void)
|
||||
{
|
||||
if (motion_enable_flag) {
|
||||
by_motion_get_speed_m1();
|
||||
// by_motion_get_speed_m2();
|
||||
|
||||
PID_Compute(&pid_m1);
|
||||
// PID_Compute(&pid_m2);
|
||||
|
||||
by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
|
||||
// by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
|
||||
}
|
||||
}
|
||||
|
||||
void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
|
||||
{
|
||||
|
||||
if (0x08 == stdd_id) {
|
||||
uint8_t mode = data[0];
|
||||
int16_t speed = (int16_t)data[1];
|
||||
// float distance = (float)(data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24));
|
||||
float distance;
|
||||
memcpy(&distance, &data[2], 4);
|
||||
if (0 == mode) {
|
||||
by_motion_set_distance2(distance, speed);
|
||||
} else if (1 == mode) {
|
||||
by_motion_set_distance(distance, speed);
|
||||
}
|
||||
}
|
||||
|
||||
// #define BC2D_MODEL1
|
||||
|
||||
// if (0x01 == stdd_id) {
|
||||
// #if defined(BC2D_MODEL1)
|
||||
// int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8));
|
||||
// int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8));
|
||||
// by_motion_set_speed_m1(speed_m1_temp);
|
||||
// by_motion_set_speed_m2(speed_m2_temp);
|
||||
// #elif defined(BC2D_MODEL2)
|
||||
// int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8));
|
||||
// int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8));
|
||||
// by_motion_set_speed_m1(speed_m1_temp);
|
||||
// by_motion_set_speed_m2(speed_m2_temp);
|
||||
// #endif
|
||||
// }
|
||||
|
||||
LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
|
||||
LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm);
|
||||
}
|
||||
|
||||
void by_motion_tmr_handle(void)
|
||||
{
|
||||
if (!motion_busy_flag) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (motion_reset_flag) {
|
||||
if (!gpio_input_data_bit_read(GPIOB, GPIO_PINS_4)) {
|
||||
motion_reset_flag = 0;
|
||||
motion_busy_flag = 0;
|
||||
by_motion_set_speed_m1(0);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (time_via > 0) {
|
||||
time_via--;
|
||||
} else {
|
||||
by_motion_set_speed_m1(0);
|
||||
motion_busy_flag = 0;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user