diff --git a/User/device/ai.c b/User/device/ai.c index 065a657..dbcf473 100644 --- a/User/device/ai.c +++ b/User/device/ai.c @@ -1,23 +1,24 @@ -#include "ai.h" +#include "device/ai.h" +#include "device/device.h" #include "bsp/uart.h" #include "component/crc16.h" +int8_t AI_Init(AI_t *ai) { + if (ai == NULL) return DEVICE_ERR_NULL; +// if (inited) return DEVICE_ERR_INITED; +// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; -// void ai_init(AI_t* ai){ -// ai->RX.head[0]="M"; -// ai->RX.head[0]="R"; -// ai->TX.head[0]="M"; -// ai->TX.head[0]="R"; -// } - - - + BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_NUC, + BSP_UART_RX_CPLT_CB); +// inited = true; + return 0; +} int8_t AI_StartReceiving(AI_t *ai) { - if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false)) - return DEVICE_OK; + if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), true)==HAL_OK) { + return DEVICE_OK;} return DEVICE_ERR; } @@ -26,9 +27,9 @@ int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) { return DEVICE_ERR; } // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16); -// if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){ -// return DEVICE_ERR; -// } + if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX))!=true){ + return DEVICE_ERR; + } ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch; ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw; ai_cmd->mode=ai->RX.mode; @@ -40,24 +41,24 @@ int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) { } -int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) { - +int8_t AI_ParseHost(AI_t* ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) { ai->TX.head[0]='M'; ai->TX.head[1]='R'; // ai->TX.mode=2; + ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle; ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle; ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed; ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed; - ai->TX.q[0]=motor->imu.quat.q1; - ai->TX.q[1]=motor->imu.quat.q2; - ai->TX.q[2]=motor->imu.quat.q3; - ai->TX.q[3]=motor->imu.quat.q0; + ai->TX.q[0]=motor->imu.quat.q0; + ai->TX.q[1]=motor->imu.quat.q1; + ai->TX.q[2]=motor->imu.quat.q2; + ai->TX.q[3]=motor->imu.quat.q3; ai->TX.bullet_count=10; - ai->TX.bullet_speed=10; + ai->TX.bullet_speed=22; - ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16); + ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t), CRC16_INIT ); if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){ return DEVICE_ERR; } @@ -65,8 +66,8 @@ int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) { } int8_t AI_StartSend(AI_t *ai) { -// if (ref_update) { - if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false)) + + if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), true)==HAL_OK) return DEVICE_OK; // else // return DEVICE_ERR; @@ -79,3 +80,86 @@ int8_t AI_StartSend(AI_t *ai) { // return DEVICE_ERR; // } } + + +// #include "ai.h" +// #include "bsp/uart.h" +// #include "component/crc16.h" + + + +// // void ai_init(AI_t* ai){ +// // ai->RX.head[0]="M"; +// // ai->RX.head[0]="R"; +// // ai->TX.head[0]="M"; +// // ai->TX.head[0]="R"; +// // } + + + + + +// int8_t AI_StartReceiving(AI_t *ai) { +// if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false)) +// return DEVICE_OK; +// return DEVICE_ERR; +// } + +// int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) { +// if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){ +// return DEVICE_ERR; +// } +// // CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16); +// // if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){ +// // return DEVICE_ERR; +// // } +// ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch; +// ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw; +// ai_cmd->mode=ai->RX.mode; +// ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc; +// ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel; +// ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc; +// ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel; +// return DEVICE_OK; +// } + + +// int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) { + +// ai->TX.head[0]='M'; +// ai->TX.head[1]='R'; +// // ai->TX.mode=2; +// ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle; +// ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle; +// ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed; +// ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed; +// ai->TX.q[0]=motor->imu.quat.q1; +// ai->TX.q[1]=motor->imu.quat.q2; +// ai->TX.q[2]=motor->imu.quat.q3; +// ai->TX.q[3]=motor->imu.quat.q0; + +// ai->TX.bullet_count=10; +// ai->TX.bullet_speed=10; + +// ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16); +// if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){ +// return DEVICE_ERR; +// } +// return DEVICE_OK; +// } + +// int8_t AI_StartSend(AI_t *ai) { +// // if (ref_update) { +// if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false)) +// return DEVICE_OK; +// // else +// // return DEVICE_ERR; +// // } else { +// // if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI), +// // (uint8_t *)&(ai->TX), +// // sizeof(ai->TX)) == HAL_OK) +// // return DEVICE_OK; +// // else +// // return DEVICE_ERR; +// // } +// } diff --git a/User/module/chassis.c b/User/module/chassis.c index 499aafd..b62a6f3 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -128,8 +128,8 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq) } // 舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面 - MotorOffset_t motor_offset = {{3.67848611 / M_PI * 180.0f, 2.61390328 / M_PI * 180.0f, - 2.11075759 / M_PI * 180.0f, 2.62694216 / M_PI * 180.0f}}; // 右右右右 + MotorOffset_t motor_offset = {{1.6467284 / M_PI * 180.0f, 5.77390385 / M_PI * 180.0f, + 1.56696141 / M_PI * 180.0f, 5.77160311 / M_PI * 180.0f}}; // 右右右右 c->motoroffset = motor_offset; /*对3508的速度环和6020的角速度以及位置环pid进行初始化*/ @@ -394,9 +394,9 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now) case CHASSIS_MODE_FOLLOW_GIMBAL: // 跟随云台模式 - c->move_vec.Vx =c_cmd->y_l; - c->move_vec.Vy =c_cmd->x_l; - c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); + c->move_vec.Vx =-c_cmd->y_l; + c->move_vec.Vy =-c_cmd->x_l; + c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 2.06f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt); break; diff --git a/User/module/cmd.c b/User/module/cmd.c index 116fd7e..fcf0275 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -142,6 +142,22 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd break; } + switch (rc->ET16s.key_B) + { + case CMD_SW_UP: + g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE; + break; + case CMD_SW_MID: + g_cmd->ctrl_mode = GIMBAL_MODE_AI; + break; + case CMD_SW_DOWN: + g_cmd->ctrl_mode = GIMBAL_MODE_AI; + break; + case CMD_SW_ERR: + g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE; + break; + } + switch (rc->ET16s.key_E) { case CMD_SW_UP: diff --git a/User/module/cmd.h b/User/module/cmd.h index 648189f..136d450 100644 --- a/User/module/cmd.h +++ b/User/module/cmd.h @@ -102,11 +102,20 @@ typedef struct GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ } Gimbal_Mode_t; + typedef enum +{ + GIMBAL_MODE_REMOTE, + GIMBAL_MODE_AI, + +}GIMBAL_Ctrl_mode_t; typedef struct { Gimbal_Mode_t mode; float delta_yaw_4310; float delta_pitch_4310; float delta_yaw_6020; + GIMBAL_Ctrl_mode_t ctrl_mode; + float set_yaw; + float set_pitch; } Gimbal_CMD_t; typedef enum { diff --git a/User/module/config.c b/User/module/config.c index 973433b..fbf5c16 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -198,7 +198,7 @@ static const Config_Param_t config = { .mech_zero = { .yaw_6020 = 1.31f, .yaw_4310 = 2.06f, /*大yaw零点*/ - .pitch_4310 = 1.0f, + .pitch_4310 = 0.93f, }, .travel = { .yaw_6020 = 1.4f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index caf91cd..230f9e9 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -201,6 +201,25 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ return GIMBAL_OK; } +// int8_t Gimbal_Control_mode(Gimbal_t *g,Gimbal_CMD_t *g_cmd){ +// if (g == NULL || g_cmd == NULL) { +// return -1; +// } +// switch(g_cmd->ctrl_mode){ +// case GIMBAL_MODE_REMOTE: +// g->setpoint.eulr.yaw=g_cmd->delta_yaw_6020; +// g->setpoint.eulr.pit=g_cmd->delta_pitch_4310; +// break; +// case GIMBAL_MODE_AI: +// g->setpoint.eulr.yaw=g_cmd->set_yaw; +// g->setpoint.eulr.pit=g_cmd->set_pitch; +// break; +// default: +// break; +// } +// return GIMBAL_OK; +// }; + /** * \brief 运行云台控制逻辑 * @@ -294,6 +313,16 @@ switch (g->mode) { case GIMBAL_MODE_ABSOLUTE: /*绝对坐标 大地坐标系下云台控制*/ /*6020小YAW控制*/ + // Gimbal_Control_mode(g, g_cmd); +if(g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE){ + g->setpoint.eulr.yaw = g->setpoint.eulr.yaw; + g->setpoint.eulr.pit = g->setpoint.eulr.pit; +} +else if (g_cmd->ctrl_mode == GIMBAL_MODE_AI) { +g->setpoint.eulr.yaw = g_cmd->set_yaw; +g->setpoint.eulr.pit = g_cmd->set_pitch; +} + yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw, g->feedback.imu.eulr.yaw,0.0f,g->dt); g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point, @@ -330,6 +359,9 @@ switch (g->mode) { return 0; } + + + /** * \brief 云台输出 * @@ -338,11 +370,12 @@ return 0; * void类型的，可以改成int * 4310为力矩输出控制，传统的电机控制方法，两者之积不要大于12.5 */ - MOTOR_MIT_Output_t output_yaw_4310 = {0}; + void Gimbal_Output(Gimbal_t *g) { MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020); + MOTOR_MIT_Output_t output_yaw_4310 = {0}; output_yaw_4310.torque = g->out.yaw_4310 * 1.0f; output_yaw_4310.kd = 0.1f; // output_yaw_4310.kd=0.1f; diff --git a/User/module/shoot.c b/User/module/shoot.c index 9e97fdd..820a986 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -173,7 +173,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) switch(s->param->basic.projectileType) { case SHOOT_PROJECTILE_17MM: - s->target_variable.fric_rpm=5000.0f; + s->target_variable.fric_rpm=6500.0f; break; case SHOOT_PROJECTILE_42MM: s->target_variable.fric_rpm=4000.0f; diff --git a/User/task/Task8.c b/User/task/Task8.c index a137390..17446e5 100644 --- a/User/task/Task8.c +++ b/User/task/Task8.c @@ -1,4 +1,4 @@ -/* + /* Task8 Task */ diff --git a/User/task/gimbal.c b/User/task/gimbal.c index 21750d0..fc0905b 100644 --- a/User/task/gimbal.c +++ b/User/task/gimbal.c @@ -8,6 +8,7 @@ /* USER INCLUDE BEGIN */ #include "module/gimbal.h" #include "module/config.h" +#include "device/ai.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -15,9 +16,13 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -Gimbal_CMD_t cmd_gimbal; +Gimbal_CMD_t cmd_gimbal; //遥控器 Gimbal_IMU_t gimbal_imu; Gimbal_t gimbal; +AI_cmd_t ai_gimbal_cmd; + +Gimbal_CMD_t final_gimbal_cmd; //最终命令 + /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -39,10 +44,39 @@ void Task_gimbal(void *argument) { while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ -osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0); -Gimbal_UpdateIMU(&gimbal, &gimbal_imu); + osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0); + Gimbal_UpdateIMU(&gimbal, &gimbal_imu); + +osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd,&ai_gimbal_cmd,NULL, 0); + /* ai指令 */ + // if(osMessageQueueGet(task_runtime.msgq.gimbal.ai.g_cmd, &ai_gimbal_cmd, NULL, 0)==osOK){ + // if(ai_gimbal_cmd.mode==0){ + // final_gimbal_cmd.set_pitch =gimbal_imu.eulr.rol; + // final_gimbal_cmd.set_yaw=gimbal_imu.eulr.yaw; + // } + // if(ai_gimbal_cmd.mode==2){ + // final_gimbal_cmd.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit; + // final_gimbal_cmd.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw; + // } + // } + + osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); + +if(ai_gimbal_cmd.mode==0){ + cmd_gimbal.set_pitch =gimbal_imu.eulr.pit; + cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw; + } + if(ai_gimbal_cmd.mode==1) + { + cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit; + cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw; + } + if(ai_gimbal_cmd.mode==2){ + cmd_gimbal.set_pitch=ai_gimbal_cmd.gimbal_t.setpoint.pit; + cmd_gimbal.set_yaw=ai_gimbal_cmd.gimbal_t.setpoint.yaw; + } Gimbal_UpdateFeedback(&gimbal); osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);