108 lines
2.8 KiB
C
108 lines
2.8 KiB
C
#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)
|
||
|
||
by_motor_param param_m1;
|
||
by_motor_param param_m2;
|
||
PID_TypeDef pid_m1;
|
||
PID_TypeDef pid_m2;
|
||
int16_t speed_m1;
|
||
int16_t speed_m2;
|
||
uint8_t motion_enable_flag;
|
||
|
||
void by_motion_set_pwm_m1(int32_t pwm_duty)
|
||
{
|
||
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
|
||
pwm_duty += 499;
|
||
|
||
// 互补 pwm 输出，499 为中值
|
||
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, -449, 449); // 不可以拉满哦
|
||
pwm_duty += 499;
|
||
|
||
// 互补 pwm 输出，499 为中值
|
||
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty);
|
||
}
|
||
|
||
int16_t by_motion_get_speed_m1(void)
|
||
{
|
||
param_m1.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR2));
|
||
tmr_counter_value_set(TMR2, 0);
|
||
return (int16_t)param_m1.real_speed;
|
||
}
|
||
|
||
int16_t by_motion_get_speed_m2(void)
|
||
{
|
||
param_m2.real_speed = (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
|
||
tmr_counter_value_set(TMR3, 0);
|
||
return (int16_t)param_m2.real_speed;
|
||
}
|
||
|
||
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, ¶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, 10);
|
||
PID_SetOutputLimits(&pid_m1, -400, 400);
|
||
|
||
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);
|
||
|
||
param_m1.target_speed = 17;
|
||
|
||
motion_enable_flag = 1;
|
||
DRV_ENABLE();
|
||
}
|
||
|
||
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);
|
||
|
||
LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
|
||
by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
|
||
// by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
|
||
}
|
||
} |