261 lines
7.1 KiB
C
261 lines
7.1 KiB
C
#include "Chassis.h"
|
||
#include "gpio.h"
|
||
#include "user_math.h"
|
||
#include "bsp_buzzer.h"
|
||
#include "bsp_delay.h"
|
||
|
||
|
||
|
||
|
||
|
||
|
||
/*机器人坐标系，向前x，右y，上yaw
|
||
不同于nuc (前x，左y，上yaw) */
|
||
/*
|
||
x
|
||
|
|
||
--y
|
||
|
||
|
||
*/
|
||
|
||
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
|
||
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
|
||
if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
|
||
//此处源代码处做了pid的reset 待添加
|
||
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
|
||
c->chassis_ctrl.mode =ctrl->CMD_mode ;
|
||
|
||
return CHASSIS_OK;
|
||
} //设置控制模式
|
||
|
||
|
||
/*该函数用来更新can任务获得的电机反馈值*/
|
||
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
if (can == NULL) return CHASSIS_ERR_NULL;
|
||
for (uint8_t i = 0; i < 4; i++)
|
||
{
|
||
c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
|
||
c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
|
||
}
|
||
|
||
|
||
c->motorfeedback.rotor_pit6020angle = can->motor.pit6020.as_array[0].rotor_ecd;
|
||
c->motorfeedback.rotor_pit6020rpm = can->motor.pit6020.as_array[0].rotor_speed;
|
||
|
||
c->motorfeedback.rotor_gimbal_yawangle = can->motor.chassis6020.as_array[0].rotor_ecd;
|
||
c->motorfeedback.rotor_gimbal_yawrpm = can->motor.chassis6020.as_array[0].rotor_speed;
|
||
|
||
c->motorfeedback.rotor_gimbal_pitchangle = can->motor.chassis6020.as_array[1].rotor_ecd;
|
||
c->motorfeedback.rotor_gimbal_pitchrpm = can->motor.chassis6020.as_array[1].rotor_speed;
|
||
|
||
// c->sick_dis[0] = can->sickfed.raw_dis[0];
|
||
// c->sick_dis[1] = can->sickfed.raw_dis[1];
|
||
// c->sick_dis[2] = can->sickfed.raw_dis[2];
|
||
// c->sick_dis[3] = can->sickfed.raw_dis[3];
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
|
||
{
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->param = param; /*初始化参数 */
|
||
|
||
for(int i =0 ; i < 4 ; i++)
|
||
{
|
||
PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param)); //带D项滤波
|
||
}
|
||
|
||
PID_init(&(c->pid.chassis_PICKWzPID_HIGN),PID_POSITION,&(c->param->chassis_PICKWzPID_HIGN_param));
|
||
|
||
PID_init(&(c->pid.chassis_PICKWzPID_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
|
||
|
||
|
||
PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
|
||
|
||
PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
|
||
|
||
LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给角加速度做滤波
|
||
|
||
|
||
LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
|
||
|
||
LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给y做滤波
|
||
|
||
LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
|
||
|
||
//
|
||
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
|
||
{
|
||
c->hopemotorout.OmniSpeedOut[3] = -Vx+Vy+Vw;//右前
|
||
c->hopemotorout.OmniSpeedOut[0] = -Vx-Vy+Vw;//右后
|
||
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
|
||
c->hopemotorout.OmniSpeedOut[1] = Vx+Vy+Vw;//左前
|
||
}
|
||
|
||
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
|
||
//{
|
||
// uint16_t xArrive = 0, yArrive = 0;
|
||
// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
|
||
// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
|
||
// if(xArrive && yArrive) return true;
|
||
// else return false;
|
||
//}
|
||
|
||
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
|
||
{
|
||
if(c ==NULL) return CHASSIS_ERR_NULL;
|
||
if(ctrl ==NULL) return CHASSIS_ERR_NULL;
|
||
|
||
|
||
Chassis_SetCtrl(c,ctrl);
|
||
|
||
//此处对imu加滤波做修正
|
||
c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
|
||
|
||
switch (c->chassis_ctrl .ctrl)
|
||
{
|
||
case RCcontrol: //手动控制
|
||
|
||
/*
|
||
在cmd里对数据进行处理 包括方向和油门
|
||
6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
|
||
if(c->chassis_ctrl .mode == Pitch){
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
}
|
||
else if(c->chassis_ctrl .mode == UP_Adjust)
|
||
{
|
||
c->move_vec.Vw = ctrl->Vw*6000;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
}
|
||
else {
|
||
c->move_vec.Vw = ctrl->Vw*6000;
|
||
c->move_vec.Vx = ctrl->Vy*6000;
|
||
c->move_vec.Vy = ctrl->Vx*6000;
|
||
}
|
||
|
||
|
||
|
||
break;
|
||
|
||
case AUTO : //在自动模式下
|
||
|
||
switch(c->chassis_ctrl.mode ){
|
||
|
||
|
||
case AUTO_NAVI: //自动雷达
|
||
// //这套是全向轮的方向,一定要注意这里的xy方向
|
||
c->move_vec.Vw =ctrl->cmd_MID360 .posw *1000 ;
|
||
c->move_vec.Vy =-ctrl->cmd_MID360.posy *1000 ;
|
||
c->move_vec.Vx =-ctrl->cmd_MID360.posx *1000 ;
|
||
// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
|
||
// c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
|
||
// c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
|
||
//
|
||
// c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
|
||
// c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
|
||
// c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
|
||
// //电机速度限幅
|
||
|
||
abs_limit_fp(&c->move_vec.Vx,2000.0f);
|
||
|
||
abs_limit_fp(&c->move_vec.Vy,2000.0f);
|
||
|
||
abs_limit_fp(&c->move_vec.Vw,2000.0f);
|
||
|
||
if(ctrl->status[5] ==1)
|
||
{
|
||
c->move_vec.Vw = c->move_vec.Vw * 0.8f;
|
||
c->move_vec.Vx = c->move_vec.Vx * 0.5f;
|
||
c->move_vec.Vy = c->move_vec.Vy * 0.5f;
|
||
}
|
||
|
||
break;
|
||
|
||
case AUTO_PICK: //自动视觉
|
||
|
||
c->move_vec.Vx =ctrl->Vx*6000 ;
|
||
c->move_vec.Vy =ctrl->Vy *6000;
|
||
c->move_vec .Vw = -ctrl->cmd_pick .posx;
|
||
|
||
|
||
|
||
if(abs_value(ctrl ->cmd_pick .posx )>20)
|
||
{
|
||
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
|
||
|
||
}
|
||
else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
|
||
{
|
||
c->move_vec.Vw =0;
|
||
}
|
||
else
|
||
c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0);
|
||
|
||
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
|
||
|
||
|
||
c->vofa_send[0]=c->move_vec.Vw;
|
||
c->vofa_send[1]=-ctrl->cmd_pick .posx;
|
||
|
||
|
||
|
||
|
||
|
||
break ;
|
||
|
||
|
||
|
||
}
|
||
break ;
|
||
|
||
}
|
||
|
||
|
||
//电机速度限幅
|
||
|
||
abs_limit_fp(&c->move_vec.Vx,6000.0f);
|
||
|
||
abs_limit_fp(&c->move_vec.Vy,6000.0f);
|
||
|
||
abs_limit_fp(&c->move_vec.Vw,6000.0f);
|
||
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
|
||
|
||
|
||
for (uint8_t i = 0 ; i <4 ; i++)
|
||
{
|
||
c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
|
||
|
||
out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
|
||
}
|
||
|
||
|
||
// c->vofa_send[0]=c->pos088.bmi088.gyro.x;
|
||
// c->vofa_send[1]=c->pos088.bmi088.gyro.y;
|
||
// c->vofa_send[2]=c->pos088.bmi088.gyro.z;
|
||
// c->vofa_send[3]=c->pos088.bmi088.accl.x;
|
||
// c->vofa_send[4]=c->pos088.bmi088.accl.y;
|
||
// c->vofa_send[5]=c->pos088.bmi088.accl.z;
|
||
|
||
return CHASSIS_OK;
|
||
|
||
}
|
||
|
||
|
||
|
||
|