btl143/BC2D-POS-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加距离控制

Browse Source
This commit is contained in:
bmy
2024-05-10 12:17:54 +08:00
parent 1c83cbc70a
commit 82e5f25e3a
4 changed files with 132 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
app/by_motion.c
View File

@@ -8,11 +8,16 @@
#define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
#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;
int16_t time_via;
float position_abs;
void by_motion_set_pwm_m1(int32_t pwm_duty)
{
@@ -23,14 +28,14 @@ void by_motion_set_pwm_m1(int32_t pwm_duty)
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}
void by_motion_set_pwm_m2(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
pwm_duty += 499;
// void by_motion_set_pwm_m2(int32_t pwm_duty)
// {
// pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦
// pwm_duty += 499;
// 互补 pwm 输出，499 为中值
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
}
// // 互补 pwm 输出，499 为中值
// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
// }
int16_t by_motion_get_speed_m1(void)
{
@@ -43,41 +48,96 @@ int16_t by_motion_get_speed_m1(void)
#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
}
// 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)
// void by_motion_set_speed_m2(int16_t speed)
// {
// param_m2.target_speed = speed;
// }
void by_motion_set_distance(float distance, int16_t speed)
{
param_m2.target_speed = speed;
if (motion_busy_flag) {
return;
}
if (speed < 0) {
speed = -speed;
}
if (distance < 0.0f) {
by_motion_set_speed_m1(-1 * speed);
distance = -distance;
} else {
by_motion_set_speed_m1(speed);
}
motion_busy_flag = 1;
position_abs += distance;
time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
// lwprintf("time_via = %d\r\n", time_via);
}
void by_motion_set_distance2(float distance, int16_t speed)
{
if (motion_busy_flag) {
return;
}
if (speed < 0) {
speed = -speed;
}
if (distance < 0.0f) {
distance = -distance;
}
distance = position_abs - distance;
if (distance < 0.0f) {
distance = -distance;
by_motion_set_speed_m1(-1 * speed);
} else {
by_motion_set_speed_m1(speed);
}
motion_busy_flag = 1;
position_abs += distance;
time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
// 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);
// 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;
// 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
@@ -87,10 +147,10 @@ void by_motion_init(void)
PID_SetSampleTime(&pid_m1, 10);
PID_SetOutputLimits(&pid_m1, -400, 400);
PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_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);
// PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_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();
@@ -112,22 +172,50 @@ void by_motion_run(void)
void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
{
#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
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 (time_via > 0) {
time_via--;
} else {
by_motion_set_speed_m1(0);
motion_busy_flag = 0;
}
}

1
app/by_motion.h
View File

@@ -28,6 +28,7 @@ extern int16_t by_motion_get_speed_m2(void);
extern void by_motion_set_speed_m1(int16_t speed);
extern void by_motion_set_speed_m2(int16_t speed);
extern void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len);
extern void by_motion_tmr_handle(void);
extern by_motor_param param_m1;
extern by_motor_param param_m2;

1
project/src/at32f413_int.c
View File

@@ -237,6 +237,7 @@ void TMR4_GLOBAL_IRQHandler(void)
/* add user code begin TMR4_GLOBAL_IRQ 0 */
if (SET == tmr_interrupt_flag_get(TMR4, TMR_OVF_FLAG)) {
by_motion_run();
by_motion_tmr_handle();
tmr_flag_clear(TMR4, TMR_OVF_FLAG);
}
/* add user code end TMR4_GLOBAL_IRQ 0 */

2
project/src/main.c
View File

@@ -129,7 +129,7 @@ int main(void)
/* add user code begin 3 */
// lwprintf("pwm value: %f\r\n", param_m1.out_pwm);
// lwprintf("mot speed: %f\r\n", param_m1.real_speed);
lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
// lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
/* add user code end 3 */
}
}