519 lines
23 KiB
C
519 lines
23 KiB
C
#include "Chassis.h"
|
||
#include "define.h"
|
||
|
||
/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处(define.h里有快捷方法)，
|
||
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
|
||
查看6020反馈值，将6020反馈值放入motor_offset中*/
|
||
|
||
|
||
//底盘初始化
|
||
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq){
|
||
c->param = param; /*初始化参数 */
|
||
|
||
//舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面
|
||
MotorOffset_t motor_offset = { {89.901093, 330.179474, 208.084482, 29.337082}};
|
||
c->motoroffset = motor_offset; // 将 motor_offset 的值赋给 c->motoroffset
|
||
|
||
//在这里修改雷达校准时sick的值
|
||
c->sick_set[0] = 2500;
|
||
c->sick_set[1] = 2500;
|
||
c->sick_set[2] = 600;
|
||
|
||
//校准初始化
|
||
c->radar_reset_flag = 0;
|
||
|
||
//蜂鸣器初始化
|
||
Buzzer_Init(&c->buzzer_radar_angle,200,1000,0.9);
|
||
Buzzer_Init(&c->buzzer_nuc_flag,200,1000,0.9);
|
||
|
||
for(int i=0;i<4;i++){
|
||
PID_init(&(c->pid.chassis_6020OmegaPid[i]), PID_POSITION,&(c->param->C6020Omega_param));
|
||
PID_init(&(c->pid.chassis_6020anglePid[i]), PID_POSITION,&(c->param->C6020Angle_param));
|
||
}
|
||
|
||
PID_init(&(c->pid.Chassis_AngleAdjust),PID_POSITION,&(c->param->Chassis_AngleAdjust_param));
|
||
|
||
for(int i=0;i<2;i++){
|
||
PID_init(&(c->pid.chassis_RadaranglePID[i]),PID_POSITION,&(c->param->RadarAngle_param));
|
||
PID_init(&(c->pid.chassis_RadarspeedPID[i]),PID_POSITION,&(c->param->RadarSpeed_param));
|
||
}
|
||
|
||
PID_init(&(c->pid.chassis_SickVx), PID_POSITION,&(c->param->SickVx_param));
|
||
PID_init(&(c->pid.chassis_SickVy), PID_POSITION,&(c->param->SickVy_param));
|
||
PID_init(&(c->pid.chassis_SickVw), PID_POSITION,&(c->param->SickVw_param));
|
||
|
||
LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给x 做滤波
|
||
LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给y 做滤波
|
||
LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给w 做滤波
|
||
LowPassFilter2p_Init(&(c->filled[3]),target_freq,8.0f); //给雷达angle做滤波
|
||
LowPassFilter2p_Init(&(c->filled[4]),target_freq,8.0f); //给雷达yaw做滤波
|
||
LowPassFilter2p_Init(&(c->filled[5]),target_freq,8.0f);
|
||
LowPassFilter2p_Init(&(c->filled[6]),target_freq,10.0f); //给sick1做滤波
|
||
LowPassFilter2p_Init(&(c->filled[7]),target_freq,10.0f); //给sick2做滤波
|
||
LowPassFilter2p_Init(&(c->filled[8]),target_freq,10.0f); //给sick3做滤波
|
||
|
||
c->set_point.yaw = 0.0f; // 初始化偏航角
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
|
||
c->mode =ctrl->C_cmd.mode;
|
||
c->pos =ctrl->C_cmd.pos;
|
||
return 0;
|
||
}
|
||
|
||
//该函数用来更新其他任务获得的数据
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
|
||
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
|
||
c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
|
||
|
||
c->motorfeedback.rotor_rpm6020[i] = can->motor.chassis6020.as_array[i].rotor_speed;
|
||
c->motorfeedback.rotor_current6020[i] = can->motor.chassis6020.as_array[i].torque_current;
|
||
c->motorfeedback.rotor_angle6020[i] = can->motor.chassis6020.as_array[i].rotor_angle;
|
||
c->motorfeedback.rotor_temp6020[i] = can->motor.chassis6020.as_array[i].temp;
|
||
|
||
#ifdef calibration
|
||
#else
|
||
//由于安装不能保证0点朝向我们想要朝向的方向，所以进行零点偏移
|
||
c->motorfeedback.rotor_angle6020[i] = fmod(can->motor.chassis6020.as_array[i].rotor_angle -
|
||
c->motoroffset.MOTOR_OFFSET[i], 360.0);
|
||
if(c->motorfeedback.rotor_angle6020[i]<0){
|
||
c->motorfeedback.rotor_angle6020[i]+=360;
|
||
}
|
||
#endif
|
||
|
||
c->motorfeedback.rotor_rpm5065[i] = can->motor.chassis5065.as_array[i].rotor_speed;
|
||
c->motorfeedback.torque_current5065[i] = can->motor.chassis5065.as_array[i].torque_current;
|
||
|
||
}
|
||
//接收sick数据
|
||
for (uint8_t i = 0; i < 3; i++) {
|
||
c->sick[i] = can->sickfed.as_array[i].sick_distance;
|
||
c->sick[i] = LowPassFilter2p_Apply(&(c->filled[i+6]),c->sick[i]);
|
||
c->sick[i] = c->sick[i]/10;
|
||
}
|
||
// 将 IMU 的 yaw 从 [+180°, 0,-180°] 转换到 [0°, 360°]
|
||
c->angle_current = - AngleChange(DEGREE, c->pos088.imu_eulr.yaw);
|
||
if (c->angle_current < 0) c->angle_current += 360.0f;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
//底盘解算
|
||
void Chassis_speed_calculate(Chassis_t *c,Action_POS_t*pos,CMD_t *ctrl) {
|
||
|
||
// RC模式下松开遥控器防止6020回到默认位置导致侧翻
|
||
if (c->mode == RC&&fabs(c->move_vec.Vx) < 3000 && fabs(c->move_vec.Vy) < 3000 && fabs(c->move_vec.Vw) < 3000) {
|
||
// 如果之前不处于保持模式，则记录当前角度
|
||
if (!c->keeping_angle_flag) {
|
||
c->keeping_angle_flag = 1; // 进入保持模式
|
||
}
|
||
|
||
// 使用保持的角度，而不是实时反馈值,速度为0
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
|
||
c->hopemotorout.rotor5065_jiesuan_1[i] = 0;
|
||
}
|
||
}
|
||
else {
|
||
// 如果有速度输入，则退出保持模式
|
||
c->keeping_angle_flag = 0;
|
||
//让保持角度实时等于进入保持阈值前的最后一次角度值
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
c->keep_angle[i] = c->hopemotorout.rotor6020_jiesuan_1[i];
|
||
}
|
||
|
||
switch(c->mode){
|
||
case RC: //RC模式下遥控器控制底盘运动，操控为机器人坐标系 x朝前，y朝右
|
||
//解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
|
||
c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
|
||
|
||
//解算得到6020角度（-180°——+180°）
|
||
c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
break;
|
||
|
||
case STOP: //停止模式下轮子锁死
|
||
for(int i =0 ; i < 4 ; i++){
|
||
c->hopemotorout.rotor5065_jiesuan_1[i]=0;
|
||
}
|
||
|
||
c->hopemotorout.rotor6020_jiesuan_1[0]=315;
|
||
c->hopemotorout.rotor6020_jiesuan_1[1]=45;
|
||
c->hopemotorout.rotor6020_jiesuan_1[2]=45;
|
||
c->hopemotorout.rotor6020_jiesuan_1[3]=315;
|
||
break;
|
||
|
||
case NAVI:
|
||
#ifdef radar
|
||
c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
|
||
(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
|
||
|
||
//解算得到6020角度（-180°——+180°）
|
||
c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
|
||
(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
|
||
break;
|
||
|
||
// //利用雷达反馈的自身角度把机器人坐标系转到世界坐标系
|
||
// c->chassis_yaw = ctrl->nuc.yaw* (M_PI / 180.0f);
|
||
// float cos_yaw = cosf(c->chassis_yaw);
|
||
// float sin_yaw = sinf(c->chassis_yaw);
|
||
//
|
||
// // 将速度从世界坐标系转换到底盘坐标系
|
||
// float Vx_local = c->move_vec.Vx * cos_yaw + c->move_vec.Vy * sin_yaw;
|
||
// float Vy_local = -c->move_vec.Vx * sin_yaw + c->move_vec.Vy * cos_yaw;
|
||
//
|
||
// //解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
|
||
// c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
|
||
// (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
|
||
// (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
|
||
//
|
||
// c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
|
||
// (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
|
||
// (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
|
||
//
|
||
// c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
|
||
// (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
|
||
// (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
|
||
//
|
||
// c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
|
||
// (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
|
||
// (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
|
||
//
|
||
// //解算得到6020角度（-180°——+180°）
|
||
// c->hopemotorout.rotor6020_jiesuan_1[0]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
|
||
// (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
// c->hopemotorout.rotor6020_jiesuan_1[1]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
|
||
// (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
// c->hopemotorout.rotor6020_jiesuan_1[2]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
|
||
// (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
// c->hopemotorout.rotor6020_jiesuan_1[3]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
|
||
// (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
// break;
|
||
#elif defined(action_sick)
|
||
if(ctrl->C_cmd.nuc_radar == WORLD && ctrl->C_cmd.dribble == Pause){
|
||
//运球时锁死
|
||
for(int i =0 ; i < 4 ; i++){
|
||
c->hopemotorout.rotor5065_jiesuan_1[i]=0;
|
||
}
|
||
|
||
c->hopemotorout.rotor6020_jiesuan_1[0]=315;
|
||
c->hopemotorout.rotor6020_jiesuan_1[1]=45;
|
||
c->hopemotorout.rotor6020_jiesuan_1[2]=45;
|
||
c->hopemotorout.rotor6020_jiesuan_1[3]=315;
|
||
break;
|
||
}
|
||
else{
|
||
//码盘sick模式下操控为世界坐标系，能实现舵轮小陀螺前进
|
||
c->chassis_yaw = pos->pos_yaw* (M_PI / 180.0f);
|
||
float cos_yaw = cosf(c->chassis_yaw);
|
||
float sin_yaw = sinf(c->chassis_yaw);
|
||
|
||
// 将速度从世界坐标系转换到底盘坐标系
|
||
float Vx_local = c->move_vec.Vx * cos_yaw + c->move_vec.Vy * sin_yaw;
|
||
float Vy_local = -c->move_vec.Vx * sin_yaw + c->move_vec.Vy * cos_yaw;
|
||
|
||
//解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
|
||
c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
|
||
(Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
|
||
(Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
|
||
(Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
|
||
(Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
|
||
(Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
|
||
(Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
|
||
|
||
c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
|
||
(Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
|
||
(Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
|
||
|
||
//解算得到6020角度（-180°——+180°）
|
||
c->hopemotorout.rotor6020_jiesuan_1[0]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
|
||
(Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[1]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
|
||
(Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[2]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
|
||
(Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
c->hopemotorout.rotor6020_jiesuan_1[3]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
|
||
(Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
|
||
break;
|
||
}
|
||
#endif
|
||
|
||
}
|
||
}
|
||
//角度归化到0°——360°
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
if(c->hopemotorout.rotor6020_jiesuan_1[i]<0){
|
||
c->hopemotorout.rotor6020_jiesuan_1[i]+=360;
|
||
}
|
||
}
|
||
|
||
//舵向电机就近转位
|
||
float angle_error[4];//角度误差
|
||
for (uint8_t i = 0; i < 4; i++) {
|
||
angle_error[i]=c->hopemotorout.rotor6020_jiesuan_1[i]-c->motorfeedback.rotor_angle6020[i];
|
||
//误差角度归化到-180°——+180°
|
||
while (angle_error[i] > 180) angle_error[i] -= 360;
|
||
while (angle_error[i] < -180) angle_error[i] += 360;
|
||
/*这里发现如果下面的c->motorfeedback.rotor_angle6020[i]+angle_error[i]变为
|
||
c->hopemotorout.rotor6020_jiesuan_1[i]会导致6020出现故障*/
|
||
if(angle_error[i]>90&&angle_error[i]<=180){
|
||
c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
|
||
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]-180;
|
||
}
|
||
else if(angle_error[i]<-90&&angle_error[i]>=-180){
|
||
c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
|
||
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]+180;
|
||
}
|
||
else{
|
||
c->hopemotorout.rotor5065_jiesuan_2[i]=c->hopemotorout.rotor5065_jiesuan_1[i];
|
||
c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i];
|
||
}
|
||
}
|
||
}
|
||
|
||
//利用c板yaw纠正底盘角度
|
||
void Chassis_AngleCompensate(Chassis_t *c,CMD_t *ctrl) {
|
||
// 将 IMU 的 yaw 从 [+180°, 0,-180°] 转换到 [0°, 360°]
|
||
c->angle_current = - AngleChange(DEGREE, c->pos088.imu_eulr.yaw);
|
||
if (c->angle_current < 0) c->angle_current += 360.0f;
|
||
CircleAdd(&c->set_point.yaw, ctrl->Vw, 360.0f);
|
||
|
||
c->yaw_out = PID_calc(&c->pid.Chassis_AngleAdjust,c->angle_current,c->set_point.yaw);
|
||
}
|
||
|
||
|
||
//void Chassis_AngleCompensate(Chassis_t *c,CMD_t *ctrl) {
|
||
// fp32 current_angle;
|
||
//
|
||
// if(c->move_vec.Vw== 0){
|
||
// c->angle_piancha=current_angle-c->angle_current;
|
||
// }
|
||
// else{
|
||
// current_angle = c->angle_current;
|
||
// c->angle_piancha=0;
|
||
// }
|
||
|
||
// c->yaw_out = -PID_calc(&c->pid.Chassis_AngleAdjust,c->angle_piancha,0);
|
||
//}
|
||
|
||
//根据陀螺仪反馈的数据来防止速度过快时侧翻
|
||
void Chassis_RolPrevent(Chassis_t *c){
|
||
/*这里pit轴rol轴根据c板安装方向*/
|
||
|
||
//后侧腾空
|
||
if(c->pos088.imu_eulr.pit > 0.1f)
|
||
c->move_vec.Vx = 20000; //往前挪
|
||
//前侧腾空
|
||
else if(c->pos088.imu_eulr.pit < -0.1f)
|
||
c->move_vec.Vx = -20000; //往后挪
|
||
|
||
//右侧腾空
|
||
if(c->pos088.imu_eulr.rol > 0.1f)
|
||
c->move_vec.Vy = -20000; //往左挪
|
||
//左侧腾空
|
||
else if(c->pos088.imu_eulr.rol < -0.1f)
|
||
c->move_vec.Vy = 20000; //往右挪
|
||
}
|
||
|
||
//nuc纠正角度（右旋偏移量为正，左旋偏移量为负）
|
||
void nuc_angle_correct(Chassis_t *c,CMD_t *ctrl){
|
||
fp32 detla_angle;
|
||
fp32 detla_yaw;
|
||
//雷达数据必须滤波，否则波形是0和正常数组成的矩形
|
||
ctrl->nuc.angle = LowPassFilter2p_Apply(&(c->filled[3]),ctrl->nuc.angle);
|
||
ctrl->nuc.yaw = LowPassFilter2p_Apply(&(c->filled[4]),ctrl->nuc.yaw);
|
||
|
||
detla_angle = PID_calc(&(c->pid.chassis_RadaranglePID[0]),ctrl->nuc.angle,0);
|
||
c->radar_angle = PID_calc(&(c->pid.chassis_RadarspeedPID[0]),c->pos088.bmi088.gyro.z,detla_angle);
|
||
|
||
|
||
detla_yaw = PID_calc(&(c->pid.chassis_RadaranglePID[1]),ctrl->nuc.yaw,0);
|
||
c->radar_yaw = PID_calc(&(c->pid.chassis_RadarspeedPID[1]),c->pos088.bmi088.gyro.z,detla_yaw);
|
||
}
|
||
|
||
//雷达运用sick来校准
|
||
void radar_sick_calibration(Chassis_t *c,CMD_t *ctrl){
|
||
|
||
fp32 diff_x = c->sick[2] - c->sick_set[2];
|
||
fp32 diff_y = c->sick[0] - c->sick_set[0];
|
||
fp32 diff_w = (c->sick[0] - c->sick[1]);
|
||
|
||
c->move_sick.Vx = (fabsf(diff_x)>SICKXY_ERROR) ? PID_calc(&(c->pid.chassis_SickVx),diff_x,0): 0;
|
||
c->move_sick.Vy = (fabsf(diff_y)>SICKXY_ERROR) ? PID_calc(&(c->pid.chassis_SickVy),diff_y,0): 0;
|
||
c->move_sick.Vw = PID_calc(&(c->pid.chassis_SickVw),diff_w*4,0); //放大偏角误差
|
||
|
||
//判断sick是否到达目标点附近
|
||
static uint8_t reach_cnt = 0;
|
||
if (fabsf(diff_x) <= SICKXY_ERROR && fabsf(diff_y) <= SICKXY_ERROR &&fabsf(diff_w)<= SICKW_ERROR) {
|
||
reach_cnt++;
|
||
if (reach_cnt >= 50) {
|
||
// c->reset_sick_flag = 1;
|
||
reach_cnt = 0;
|
||
}
|
||
}
|
||
else {
|
||
// c->reset_sick_flag = 0;
|
||
}
|
||
}
|
||
|
||
//底盘控制
|
||
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out, Action_POS_t*pos){
|
||
fp32 chassis6020_detangle[4];
|
||
|
||
Chassis_SetCtrl(c,ctrl);
|
||
|
||
//陀螺仪yaw纠正底盘偏角
|
||
// Chassis_AngleCompensate(c,ctrl);
|
||
#ifdef radar
|
||
//关于nuc纠正的函数都要一直运行
|
||
nuc_angle_correct(c,ctrl);
|
||
// radar_sick_calibration(c,ctrl);
|
||
#endif
|
||
switch (c->mode){
|
||
case RC:
|
||
c->move_vec.Vx = ctrl->Vx*ctrl->throttle*9500;
|
||
c->move_vec.Vy = ctrl->Vy*ctrl->throttle*9500;
|
||
c->move_vec.Vw = ctrl->Vw*15000;
|
||
BSP_PWM_Stop(BSP_PWM_BUZZER);
|
||
break;
|
||
|
||
case STOP:
|
||
c->move_vec.Vx =0;
|
||
c->move_vec.Vy =0;
|
||
c->move_vec.Vw =0;
|
||
break;
|
||
|
||
case NAVI:
|
||
switch (ctrl->C_cmd.nuc_radar){
|
||
case ANGLE:
|
||
c->move_vec.Vx = ctrl->Vx*95000;
|
||
c->move_vec.Vy = ctrl->Vy*95000;
|
||
#ifdef nuc_1
|
||
c->move_vec.Vw = c->radar_angle;
|
||
Buzzer_Control(&c->buzzer_radar_angle,(fabsf(ctrl->nuc.angle) < 1 && ctrl->nuc.angle != 0));
|
||
#elif defined(nuc_2)
|
||
if(ctrl->C_cmd.communicate == NO){
|
||
c->move_vec.Vw = c->radar_angle;
|
||
Buzzer_Control(&c->buzzer_radar_angle,(fabsf(ctrl->nuc.angle) < 1 && ctrl->nuc.angle != 0));
|
||
}
|
||
else if(ctrl->C_cmd.communicate == YES){
|
||
c->move_vec.Vw = c->radar_yaw;
|
||
Buzzer_Control(&c->buzzer_radar_angle,(fabsf(ctrl->nuc.yaw) < 1 && ctrl->nuc.yaw != 0));
|
||
}
|
||
#endif
|
||
break;
|
||
|
||
case WORLD:
|
||
#ifdef action_sick
|
||
//跑点
|
||
if(ctrl->C_cmd.dribble == RUNNING){
|
||
c->move_vec.Vx =ctrl->C_navi.vx ;
|
||
c->move_vec.Vy =ctrl->C_navi.vy ;
|
||
c->move_vec.Vw =ctrl->C_navi.wz ;
|
||
}
|
||
//运球锁死
|
||
else if(ctrl->C_cmd.dribble == Pause){
|
||
c->move_vec.Vx =0;
|
||
c->move_vec.Vy =0;
|
||
c->move_vec.Vw =0;
|
||
}
|
||
|
||
#else
|
||
c->move_vec.Vx = ctrl->Vx*ctrl->throttle*9500;
|
||
c->move_vec.Vy = ctrl->Vy*ctrl->throttle*9500;
|
||
c->move_vec.Vw = ctrl->Vw*15000;
|
||
#endif
|
||
break;
|
||
case RADAR_RESET:
|
||
// c->move_vec.Vx = c->move_sick.Vx;
|
||
// c->move_vec.Vy = c->move_sick.Vy;
|
||
// c->move_vec.Vw = c->move_sick.Vw;
|
||
c->radar_reset_flag = 1;
|
||
if(ctrl->nuc.flag == 1) Buzzer_Control(&c->buzzer_nuc_flag,ctrl->nuc.flag == 1);
|
||
break;
|
||
}
|
||
break;
|
||
}
|
||
if(c->mode != NAVI||ctrl->C_cmd.nuc_radar != NAVI) c->radar_reset_flag = 0;
|
||
//防翻
|
||
Chassis_RolPrevent(c);
|
||
|
||
//进行滤波
|
||
c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[0]),c->move_vec.Vx);
|
||
c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vy);
|
||
c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vw);
|
||
|
||
Chassis_speed_calculate(c,pos,ctrl);
|
||
#ifdef calibration
|
||
#else
|
||
for (uint8_t i = 0 ; i <4 ; i++){
|
||
|
||
c->hopemotorout.motor6020_target[i]=c->hopemotorout.rotor6020_jiesuan_2[i];
|
||
chassis6020_detangle[i] = PID_calc(&(c->pid.chassis_6020anglePid[i]),c->motorfeedback.rotor_angle6020[i],
|
||
c->hopemotorout.motor6020_target[i]);
|
||
c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid[i]),c->motorfeedback.rotor_rpm6020[i],
|
||
chassis6020_detangle[i]);
|
||
out->chassis6020.as_array[i] = c->final_out.final_6020out[i];
|
||
|
||
c->hopemotorout.motor5065_target[i]=c->hopemotorout.rotor5065_jiesuan_2[i];
|
||
c->final_out.final_5065out[i]=c->hopemotorout.motor5065_target[i];
|
||
out->chassis5065.erpm[i] = c->final_out.final_5065out[i];
|
||
}
|
||
#endif
|
||
//vofa发送
|
||
c->vofa_send[0] =ctrl->nuc.angle;
|
||
// c->vofa_send[1] =;
|
||
// c->vofa_send[2] =;
|
||
// c->vofa_send[3] =;
|
||
// c->vofa_send[4] =;
|
||
// c->vofa_send[5] =;
|
||
// c->vofa_send[6] =;
|
||
// c->vofa_send[7] =;
|
||
return CHASSIS_OK;
|
||
}
|