231 lines
7.6 KiB
C
231 lines
7.6 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;
|
||
c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType;
|
||
c->chassis_ctrl.mode = ctrl->CMD_mode;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
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;
|
||
}
|
||
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));
|
||
}
|
||
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滤波
|
||
|
||
c->sick_cali .sickparam=c->param ->sickparam ;
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
fp32 gyro_kp = 1.0f;
|
||
fp32 PIAN = 0;
|
||
|
||
void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
|
||
fp64 Nor_Vx, Nor_Vy;
|
||
normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
|
||
|
||
c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw; // 右前
|
||
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw; // 右后
|
||
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw; // 左后
|
||
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw; // 左前
|
||
}
|
||
float aaaaa=0;
|
||
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为全向轮的倍率,遥控器坐标系和机器人坐标系不同
|
||
*/
|
||
c->NUC_send .send [0]=0;
|
||
c->NUC_send .send [1]=0;
|
||
c->NUC_send .send [2]=10;
|
||
c->NUC_send .send [3]=100;
|
||
|
||
switch (c->chassis_ctrl.mode) {
|
||
case Normal:
|
||
|
||
if(ctrl->Vw){
|
||
c->move_vec.Vw = ctrl->Vw * 6000;
|
||
c->move_vec.Vx = ctrl->Vy * 6000;
|
||
c->move_vec.Vy = ctrl->Vx * 6000;
|
||
}
|
||
else
|
||
{
|
||
c->move_vec.Vw = ctrl->Vw * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa;
|
||
c->move_vec.Vx = ctrl->Vy * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
|
||
c->move_vec.Vy = ctrl->Vx * 6000 + c->pos088 .bmi088 .gyro .z * aaaaa ;
|
||
|
||
|
||
|
||
}
|
||
|
||
break;
|
||
case Pitch:
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
break;
|
||
case UP_Adjust:
|
||
c->move_vec.Vw = ctrl->Vw * 6000;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
break;
|
||
default:
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case AUTO: // 自动模式
|
||
switch (c->chassis_ctrl.mode) {
|
||
|
||
case AUTO_MID360: // 自动雷达
|
||
// 全向轮方向, 注意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;
|
||
|
||
abs_limit_fp(&c->move_vec.Vx, 5000.0f);
|
||
abs_limit_fp(&c->move_vec.Vy, 5000.0f);
|
||
abs_limit_fp(&c->move_vec.Vw, 5000.0f);
|
||
|
||
c->NUC_send .send [0]=0;
|
||
break;
|
||
|
||
case AUTO_MID360_Pitch:
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vy = 0;
|
||
c->move_vec.Vx = 0;
|
||
|
||
c->NUC_send .send [0]=0;
|
||
break;
|
||
case AUTO_MID360_Adjust:
|
||
c->move_vec.Vw = ctrl->Vw * 6000;
|
||
c->move_vec.Vx = ctrl->Vy * 6000;
|
||
c->move_vec.Vy = ctrl->Vx * 6000;
|
||
|
||
c->NUC_send .send [0]=1;
|
||
break;
|
||
default:
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
c->NUC_send .send [0]=0;
|
||
|
||
break;
|
||
}
|
||
break;
|
||
|
||
default:
|
||
c->move_vec.Vw = 0;
|
||
c->move_vec.Vx = 0;
|
||
c->move_vec.Vy = 0;
|
||
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->NUC_send .send [1]=1;
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
|
||
/*
|
||
sick0,左1
|
||
sick1,左2
|
||
sick2,下1
|
||
|
||
|
||
*/
|
||
|
||
//int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out)
|
||
//{
|
||
//// int32_t x_set;
|
||
//// int32_t y_set;
|
||
// if (c == NULL) return CHASSIS_ERR_NULL;
|
||
// if (ctrl == NULL) return CHASSIS_ERR_NULL;
|
||
//
|
||
// //yaw修正
|
||
// if(fabs((fp32)c->sick_cali .sick_dis [0]-c->sick_cali .sick_dis [1])>(fp32)c->sick_cali .sickparam.w_error){
|
||
// c->move_vec .Vw = PID_calc(&(c->pid .chassis_SickVxPID ),c->sick_cali .sick_dis [0],c->sick_cali .sick_dis [1]);
|
||
// }
|
||
|
||
// else { //大于误差
|
||
// if(fabs((fp32)c->sick_cali .sick_dis[0]-c->sick_cali .sick_dis[1])>c->sick_cali .sickparam .xy_error)
|
||
// {//xy修正
|
||
// c->move_vec .Vx =PID_calc (&(c->pid .chassis_SickVxPID ),c->sick_cali .sick_dis [0], c->sick_cali .sickparam.x_set);
|
||
// c->move_vec .Vy =PID_calc (&(c->pid .chassis_SickVyPID),c->sick_cali .sick_dis [2], c->sick_cali .sickparam.y_set);
|
||
// }
|
||
// else {//修正OK后，给nuc返回值
|
||
//
|
||
//
|
||
// }
|
||
//
|
||
//
|
||
// }
|
||
//
|
||
//}
|
||
|
||
|
||
|
||
|