添加ai
This commit is contained in:
parent
b8698147d5
commit
ee435d8001
@ -187,7 +187,7 @@ standard names. */
|
||||
|
||||
/* USER CODE BEGIN Defines */
|
||||
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
|
||||
#define configAPPLICATION_ALLOCATED_HEAP 1
|
||||
// #define configAPPLICATION_ALLOCATED_HEAP 1
|
||||
/* USER CODE END Defines */
|
||||
|
||||
#endif /* FREERTOS_CONFIG_H */
|
||||
|
||||
@ -89,66 +89,18 @@ error:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
// void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
// cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
|
||||
// cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
|
||||
// cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
|
||||
// cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
|
||||
// cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
|
||||
// cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
|
||||
// }
|
||||
|
||||
// int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
|
||||
// if (ai == NULL) return DEVICE_ERR_NULL;
|
||||
// if (cmd_host == NULL) return DEVICE_ERR_NULL;
|
||||
// ai->ai_online = false;
|
||||
// memset(&(ai->from_host), 0, sizeof(ai->from_host));
|
||||
// memset(cmd_host, 0, sizeof(*cmd_host));
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
|
||||
// int8_t AI_PackMCU(AI_t *ai, const AI_Protucol_UpDataMCU_t *data){
|
||||
// if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
|
||||
// ai->to_host.mcu.id = AI_ID_MCU;
|
||||
// memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
|
||||
// sizeof(*quat));
|
||||
// ai->to_host.mcu.package.data.notice = 0;
|
||||
// if (ai->status == AI_STATUS_AUTOAIM)
|
||||
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
|
||||
// else if (ai->status == AI_STATUS_HITSWITCH)
|
||||
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
|
||||
// else if (ai->status == AI_STATUS_AUTOMATIC)
|
||||
// ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
|
||||
|
||||
// ai->to_host.mcu.package.crc16 = CRC16_Calc(
|
||||
// (const uint8_t *)&(ai->to_host.mcu.package),
|
||||
// sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
|
||||
// ai->to_host.mcu.package = *data;
|
||||
// ai->to_host.mcu.crc16 =
|
||||
// CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2);
|
||||
// return DEVICE_OK;
|
||||
// }
|
||||
|
||||
// int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
|
||||
// (void)ref;
|
||||
// ai->to_host.ref.id = AI_ID_REF;
|
||||
// ai->to_host.ref.package.crc16 = CRC16_Calc(
|
||||
// (const uint8_t *)&(ai->to_host.ref.package),
|
||||
// sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
|
||||
// return DEVICE_OK;
|
||||
// }
|
||||
int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
|
||||
|
||||
// int8_t AI_StartSend(AI_t *ai, bool ref_update) {
|
||||
// if (ref_update) {
|
||||
// if (HAL_UART_Transmit_DMA(
|
||||
// BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
|
||||
// sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
|
||||
// return DEVICE_OK;
|
||||
// else
|
||||
// return DEVICE_ERR;
|
||||
// } else {
|
||||
// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
|
||||
// (uint8_t *)&(ai->to_host.mcu),
|
||||
// sizeof(ai->to_host.mcu)) == HAL_OK)
|
||||
// return DEVICE_OK;
|
||||
// else
|
||||
// return DEVICE_ERR;
|
||||
// }
|
||||
// }
|
||||
int8_t AI_HandleOffline(AI_t *ai);
|
||||
|
||||
int8_t AI_StartSend(AI_t *ai, bool ref_online);
|
||||
|
||||
|
||||
@ -20,6 +20,9 @@ extern "C" {
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
#define AI_ID_MCU (0xC4)
|
||||
#define AI_ID_REF (0xA8)
|
||||
#define AI_ID_AI (0xA1)
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/* 电控 -> 视觉 MCU数据结构体*/
|
||||
|
||||
@ -284,6 +284,8 @@ void Chassis_Output(Chassis_t *c) {
|
||||
LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
|
||||
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
|
||||
c->output.wheel[1]);
|
||||
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,
|
||||
// 0.0f); // 暂时不让轮子动
|
||||
}
|
||||
|
||||
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
@ -385,7 +387,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
|
||||
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
|
||||
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
|
||||
Delta_Tp[1] = -Delta_Tp[0];
|
||||
Delta_Tp[1] = c->vmc_[1].leg.L0 * 10.0f *
|
||||
PID_Calc(&c->pid.tp[1], c->vmc_[0].leg.theta,
|
||||
c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
|
||||
|
||||
// 左腿控制
|
||||
{
|
||||
@ -398,12 +402,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
virtual_force[0] =
|
||||
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
|
||||
pid_output + 60.0f + roll_compensation_force;
|
||||
// virtual_force[0] =
|
||||
// (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
|
||||
// pid_output + 60.0f;
|
||||
|
||||
// VMC逆解算（包含摆角补偿）
|
||||
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
|
||||
c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
|
||||
// if (VMC_InverseSolve(&c->vmc_[0], 0.0f,
|
||||
// Delta_Tp[0]) == 0) {
|
||||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
|
||||
&c->output.joint[1]);
|
||||
} else {
|
||||
@ -424,13 +428,12 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
virtual_force[1] =
|
||||
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
|
||||
pid_output + 65.0f - roll_compensation_force;
|
||||
// virtual_force[1] =
|
||||
// (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
|
||||
// pid_output + 65.0f;
|
||||
|
||||
// VMC逆解算（包含摆角补偿）
|
||||
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
|
||||
c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
|
||||
// if (VMC_InverseSolve(&c->vmc_[1], 0.0f,
|
||||
// Delta_Tp[1]) == 0) {
|
||||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
|
||||
&c->output.joint[2]);
|
||||
} else {
|
||||
|
||||
@ -100,7 +100,7 @@ Config_RobotParam_t robot_config = {
|
||||
.yaw_omega = {
|
||||
.k = 1.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.5f,
|
||||
.i = 0.3f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
@ -108,7 +108,7 @@ Config_RobotParam_t robot_config = {
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw_angle = {
|
||||
.k = 10.0f,
|
||||
.k = 8.0f,
|
||||
.p = 3.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
@ -200,8 +200,8 @@ Config_RobotParam_t robot_config = {
|
||||
.range = -1.0f,
|
||||
},
|
||||
.leg_theta={
|
||||
.k=1.0f,
|
||||
.p=5.0f, /* 摆角比例系数 */
|
||||
.k=5.0f,
|
||||
.p=2.0f, /* 摆角比例系数 */
|
||||
.i=0.0f, /* 摆角积分系数 */
|
||||
.d=0.0f, /* 摆角微分系数 */
|
||||
.i_limit=0.0f, /* 积分限幅 */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user