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-04-26 16:13:07 +08:00
parent 05cab94699
commit 6bac98da0a
12 changed files with 303 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
app/by_motion.c
View File

@@ -1,15 +1,20 @@
#include "by_motion.h"
#include "pid.h"
#include "dwt_delay.h"
#include "by_utils.h"
#include "by_debug.h"
#define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
int16_t speed_m1;
int16_t speed_m2;
by_motor_param param_m1;
by_motor_param param_m2;
PID_TypeDef pid_m1;
PID_TypeDef pid_m2;
uint8_t motion_enable_flag;
void by_motion_pwm_m1(int32_t pwm_duty)
void by_motion_set_pwm_m1(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
@@ -18,7 +23,7 @@ void by_motion_pwm_m1(int32_t pwm_duty)
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}
void by_motion_pwm_m2(int32_t pwm_duty)
void by_motion_set_pwm_m2(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
@@ -29,21 +34,100 @@ void by_motion_pwm_m2(int32_t pwm_duty)
int16_t by_motion_get_speed_m1(void)
{
speed_m1 = (int16_t)tmr_counter_value_get(TMR2);
#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(TMR2));
last_speed = param_m1.real_speed;
tmr_counter_value_set(TMR2, 0);
return speed_m1;
return (int16_t)param_m1.real_speed;
#undef alpha
}
int16_t by_motion_get_speed_m2(void)
{
speed_m2 = (int16_t)tmr_counter_value_get(TMR3);
#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 speed_m2;
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_init(void)
{
motion_enable_flag = 0;
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, &param_m1.real_speed, &param_m1.out_pwm, &param_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, 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);
motion_enable_flag = 1;
DRV_ENABLE();
by_motion_pwm_m1(-125);
by_motion_pwm_m2(0);
}
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)
{
#define BC2D_MODEL2
if (0x01 == stdd_id) {
#if defined(BC2D_MODEL1)
int16_t speed_m1_temp = (int16_t)data[0] | ((int16_t)data[1] << 8);
int16_t speed_m2_temp = (int16_t)data[2] | ((int16_t)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);
}