61 lines
2.4 KiB
C
61 lines
2.4 KiB
C
|
||
/*
|
||
底盘任务
|
||
|
||
*/
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "chassis.h"
|
||
#include "device/motor_rm.h"
|
||
|
||
/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
|
||
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
|
||
查看6020反馈值，将6020反馈值放入motor_offset中*/
|
||
|
||
/*chassis_t结构体中的内容现在有 move_vec (最终输出速度)
|
||
hopemotorout(期望的底盘输出值)舵轮解算出的各个电机的期望输出值 包括四个6020 和四个3508
|
||
final_out;(经PID计算后的实际发送给电机的实时输出值) 四个6020 四个3508
|
||
motorfeedback(电机反馈数据) 四个3508 四个6020
|
||
pid 各个电机的pid参数
|
||
ChassisImu_t pos088; 088的实时姿态
|
||
MotorOffset_t motoroffset; //5065校准数据
|
||
const Chassis_Param_t *param; //一些固定的参数
|
||
fp32 vofa_send[8]; //vofa输出数据
|
||
LowPassFilter2p_t filled[9]; //低通滤波器
|
||
float keep_angle[4]; // 保持的 6020 角度
|
||
*/
|
||
|
||
/*底盘初始化*/
|
||
|
||
|
||
|
||
int8_t chassis_init(Chassis_t *chassis ,const Chassis_Param_t *param, float target_freq )
|
||
{
|
||
/*注册电机*/
|
||
for(int i=0;i<4;i++){
|
||
MOTOR_RM_Register(&);
|
||
}
|
||
chassis->param =param;
|
||
//舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
|
||
MotorOffset_t motor_offset = { {271.329506, 330.179474, 208.084482, 29.337082}};
|
||
chassis->motoroffset = motor_offset;
|
||
/*对3508的速度环和6020的角速度以及位置环pid进行初始化*/
|
||
for(int i=0;i<4;i++)
|
||
{
|
||
PID_Init(&chassis->pid.chassis_3508VPID[i], KPID_MODE_NO_D,target_freq,&chassis->param->M3508_param);
|
||
PID_Init(&chassis->pid.chassis_6020OmegaPid[i], KPID_MODE_NO_D,target_freq,&chassis->param->C6020Omega_param);
|
||
PID_Init(&chassis->pid.chassis_6020anglePid[i], KPID_MODE_NO_D,target_freq,&chassis->param->C6020Angle_param);
|
||
}
|
||
/*对遥控器的xyw进行低通滤波*/
|
||
LowPassFilter2p_Init(&chassis->filled[0], target_freq, 20.0f); // vx
|
||
LowPassFilter2p_Init(&chassis->filled[1], target_freq, 20.0f); // vy
|
||
LowPassFilter2p_Init(&chassis->filled[2], target_freq, 20.0f); // vw
|
||
|
||
chassis->set_point.yaw=0.0f;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
|
||
|
||
|