/*
 底盘任务

*/
/* 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;
}