136 lines
4.4 KiB
C
136 lines
4.4 KiB
C
/*
|
|
* 英雄履带控制
|
|
*/
|
|
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include "track.h"
|
|
#include "bsp/mm.h"
|
|
#include "bsp/time.h"
|
|
#include "component/filter/filter.h"
|
|
#include "component/user_math.h"
|
|
#include "module/chassis.h"
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/* Private macro ------------------------------------------------------------ */
|
|
#define WHEEL_RADIUS 0.03f //驱动轮半径，单位米
|
|
#define TRACK_RADIUS 1.0f //履带驱动轮半径，单位米
|
|
#define TRACK_CMD_MOTOR_jiansubi 10
|
|
/* Private variables -------------------------------------------------------- */
|
|
|
|
/* Private function -------------------------------------------------------- */
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
uint8_t Track_Init(Track_t *t, Track_Params_t *param, float target_freq)
|
|
{
|
|
if (t == NULL || param == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
t->param = param;
|
|
|
|
BSP_FDCAN_Init();
|
|
// 注册履带电机到CAN总线
|
|
for (int i = 0; i < TRACK_MOTOR_NUM; i++) {
|
|
MOTOR_DM_Register(&(t->param->motor[i]));
|
|
}
|
|
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
PID_Init(&t->pid[i], KPID_MODE_CALC_D, target_freq, ¶m->pid[i]);
|
|
}
|
|
PID_Init(&t->pid_w, KPID_MODE_CALC_D, target_freq, ¶m->pid_w);
|
|
|
|
MOTOR_DM_Enable(&t->param->motor[0]);
|
|
MOTOR_DM_Enable(&t->param->motor[1]);
|
|
return TRACK_OK;
|
|
}
|
|
|
|
uint8_t Track_Control(Track_t *t, Track_CMD_t *cmd)
|
|
{
|
|
if (t == NULL || cmd == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
t->timer.now = BSP_TIME_Get_us()/1000000.0f;
|
|
t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
|
|
t->timer.lask_wakeup = BSP_TIME_Get_us();
|
|
|
|
t->middle_val.w=PID_Calc(&t->pid_w, 0, t->middle_val.seata, 0.0f, t->timer.dt);
|
|
|
|
float fb=0;
|
|
t->middle_val.mix_motor[0]=cmd->vel + 0.5f * t->middle_val.w;
|
|
t->middle_val.mix_motor[1]=cmd->vel + 0.5f * t->middle_val.w;
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
fb=t->motor[i].motor.feedback.rotor_speed/TRACK_CMD_RPM_MAX/TRACK_CMD_MOTOR_jiansubi;
|
|
t->output.currentOut[i].torque = TRACK_CMD_MOTOR_jiansubi
|
|
*PID_Calc(&t->pid[i], t->middle_val.mix_motor[i], fb, 0.0f, t->timer.dt);
|
|
}
|
|
return TRACK_OK;
|
|
}
|
|
|
|
uint8_t Track_AutoControl(Track_t *t, const Chassis_t *chassis)
|
|
{
|
|
if (t == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
t->timer.now = BSP_TIME_Get_us()/1000000.0f;
|
|
t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
|
|
t->timer.lask_wakeup = BSP_TIME_Get_us();
|
|
if (chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL||chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL_35)
|
|
{
|
|
float target_rpm[2];
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
float rpm = (chassis->feedback.motor[i].rotor_speed + chassis->feedback.motor[i+1].rotor_speed)/2;
|
|
target_rpm[i] = rpm*(WHEEL_RADIUS/TRACK_RADIUS);
|
|
t->output.currentOut[i].torque = TRACK_CMD_MOTOR_jiansubi
|
|
*PID_Calc(&t->pid[i], target_rpm[i], t->motor[i].motor.feedback.rotor_speed, 0.0f, t->timer.dt);
|
|
}
|
|
} else {
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
t->output.currentOut[i].torque = 0.0f;
|
|
}
|
|
PID_Reset(t->pid);
|
|
}
|
|
|
|
return TRACK_OK;
|
|
}
|
|
|
|
uint8_t Track_UpdateFeedback(Track_t *t)
|
|
{
|
|
if (t == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
MOTOR_DM_Update(&t->param->motor[i]);
|
|
MOTOR_DM_t *rm_motor = MOTOR_DM_GetMotor(&t->param->motor[i]);
|
|
if (rm_motor != NULL) {
|
|
t->motor[i] = *rm_motor;
|
|
} else {
|
|
// 电机离线或未注册，保持上次数据
|
|
return TRACK_ERR_NULL;
|
|
}
|
|
}
|
|
return TRACK_OK;
|
|
}
|
|
|
|
uint8_t Track_Output(Track_t *t) {
|
|
if (t == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
// 设置所有履带电机的输出
|
|
for (int i = 0; i < TRACK_MOTOR_NUM; i++) {
|
|
// MOTOR_DM_SetOutput(&t->param->motor[i], t->output.currentOut[i]);
|
|
MOTOR_DM_MITCtrl(&t->param->motor[i],&t->output.currentOut[i]);
|
|
// MOTOR_DM_Relax(&t->param->motor[i]);
|
|
}
|
|
return TRACK_OK;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|