diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 5a289a6..4412108 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -63,9 +63,9 @@ void ChassisInit() { .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .speed_PID = { - .Kp = 1.0, // 4.5 - .Ki = 0, // 1.5 - .Kd = 0, // 0 + .Kp = 4.0f, // 4.5 + .Ki = 8.0f, // 1.5 + .Kd = 0.0015, // 0 .IntegralLimit = 3000, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .MaxOut = 12000, @@ -221,7 +221,7 @@ void ChassisTask() { chassis_cmd_recv.wz = 0; break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); + chassis_cmd_recv.wz = 10.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 chassis_cmd_recv.wz = 4000; @@ -233,10 +233,10 @@ void ChassisTask() { // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东) static float sin_theta, cos_theta; -// cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); -// sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); - sin_theta = 0; - cos_theta = 1; + cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); + sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD); +// sin_theta = 0; +// cos_theta = 1; chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 44c930b..cc54ecf 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -47,8 +47,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 -void RobotCMDInit() -{ +void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 vision_recv_data = VisionInit(&huart1); // 视觉通信串口 @@ -83,8 +82,7 @@ void RobotCMDInit() * 单圈绝对角度的范围是0~360,说明文档中有图示 * */ -static void CalcOffsetAngle() -{ +static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 @@ -109,15 +107,13 @@ static void CalcOffsetAngle() * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * */ -static void RemoteControlSet() -{ +static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - } - else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 { chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; @@ -130,22 +126,24 @@ static void RemoteControlSet() // ... } // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 - if (switch_is_down(rc_data[TEMP].rc.switch_left) )//|| vision_recv_data->target_state == NO_TARGET + if (switch_is_down(rc_data[TEMP].rc.switch_left))//|| vision_recv_data->target_state == NO_TARGET { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; + gimbal_cmd_send.yaw -= 0.0025f * (float) rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch -= 0.001f * (float) rc_data[TEMP].rc.rocker_l1; + + if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; + if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; } // 云台软件限位 - // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 - chassis_cmd_send.vy = 8.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 + chassis_cmd_send.vx = 8.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 8.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 ; // 弹舱舵机控制,待添加servo_motor模块,开启 - else - ; // 弹舱舵机控制,待添加servo_motor模块,关闭 + else; // 弹舱舵机控制,待添加servo_motor模块,关闭 // 摩擦轮控制,拨轮向上打为负,向下为正 if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 @@ -165,83 +163,82 @@ static void RemoteControlSet() * @brief 输入为键鼠时模式和控制量设置 * */ -static void MouseKeySet() -{ +static void MouseKeySet() { chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测 chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300; - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; + gimbal_cmd_send.yaw += (float) rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 + gimbal_cmd_send.pitch += (float) rc_data[TEMP].mouse.y / 660 * 10; switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 { - case 0: - shoot_cmd_send.bullet_speed = 15; - break; - case 1: - shoot_cmd_send.bullet_speed = 18; - break; - default: - shoot_cmd_send.bullet_speed = 30; - break; + case 0: + shoot_cmd_send.bullet_speed = 15; + break; + case 1: + shoot_cmd_send.bullet_speed = 18; + break; + default: + shoot_cmd_send.bullet_speed = 30; + break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 { - case 0: - shoot_cmd_send.load_mode = LOAD_STOP; - break; - case 1: - shoot_cmd_send.load_mode = LOAD_1_BULLET; - break; - case 2: - shoot_cmd_send.load_mode = LOAD_3_BULLET; - break; - default: - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; - break; + case 0: + shoot_cmd_send.load_mode = LOAD_STOP; + break; + case 1: + shoot_cmd_send.load_mode = LOAD_1_BULLET; + break; + case 2: + shoot_cmd_send.load_mode = LOAD_3_BULLET; + break; + default: + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 { - case 0: - shoot_cmd_send.lid_mode = LID_OPEN; - break; - default: - shoot_cmd_send.lid_mode = LID_CLOSE; - break; + case 0: + shoot_cmd_send.lid_mode = LID_OPEN; + break; + default: + shoot_cmd_send.lid_mode = LID_CLOSE; + break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 { - case 0: - shoot_cmd_send.friction_mode = FRICTION_OFF; - break; - default: - shoot_cmd_send.friction_mode = FRICTION_ON; - break; + case 0: + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.friction_mode = FRICTION_ON; + break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 { - case 0: - chassis_cmd_send.chassis_speed_buff = 40; - break; - case 1: - chassis_cmd_send.chassis_speed_buff = 60; - break; - case 2: - chassis_cmd_send.chassis_speed_buff = 80; - break; - default: - chassis_cmd_send.chassis_speed_buff = 100; - break; + case 0: + chassis_cmd_send.chassis_speed_buff = 40; + break; + case 1: + chassis_cmd_send.chassis_speed_buff = 60; + break; + case 2: + chassis_cmd_send.chassis_speed_buff = 80; + break; + default: + chassis_cmd_send.chassis_speed_buff = 100; + break; } switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量 { - case 1: + case 1: - break; + break; - default: + default: - break; + break; } } @@ -252,8 +249,7 @@ static void MouseKeySet() * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ -static void EmergencyHandler() -{ +static void EmergencyHandler() { // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { @@ -266,8 +262,7 @@ static void EmergencyHandler() LOGERROR("[CMD] emergency stop!"); } // 遥控器右侧开关为[上],恢复正常运行 - if (switch_is_up(rc_data[TEMP].rc.switch_right)) - { + if (switch_is_up(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON; LOGINFO("[CMD] reinstate, robot ready"); @@ -275,11 +270,10 @@ static void EmergencyHandler() } /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ -void RobotCMDTask() -{ +void RobotCMDTask() { // 从其他应用获取回传数据 #ifdef ONE_BOARD - SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data); + SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data); #endif // ONE_BOARD #ifdef GIMBAL_BOARD chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm); @@ -303,12 +297,12 @@ void RobotCMDTask() // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 #ifdef ONE_BOARD - PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send); + PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send); #endif // ONE_BOARD #ifdef GIMBAL_BOARD CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); #endif // GIMBAL_BOARD - PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); - PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); + PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send); + PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); VisionSend(&vision_send_data); } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index e1a070f..153d235 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -14,82 +14,83 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者 static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 +#include "vofa.h" -void GimbalInit() -{ + +void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW Motor_Init_Config_s yaw_config = { - .can_init_config = { - .can_handle = &hcan1, - .tx_id = 1, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 8, // 8 - .Ki = 0, - .Kd = 0, - .DeadBand = 0.1, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, + .can_init_config = { + .can_handle = &hcan1, + .tx_id = 1, + }, + .controller_param_init_config = { + .angle_PID = { + .Kp = 0.5, // 8 + .Ki = 0, + .Kd = 0, + .DeadBand = 0.1, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 100, - .MaxOut = 500, - }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 200, // 200 - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 3000, - .MaxOut = 20000, - }, + .MaxOut = 500, + }, + .speed_PID = { + .Kp = 12000, // 50 + .Ki = 3000, // 200 + .Kd = 0, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 3000, + .MaxOut = 20000, + }, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020}; + // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 + .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], + }, + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = GM6020}; // PITCH Motor_Init_Config_s pitch_config = { - .can_init_config = { - .can_handle = &hcan2, - .tx_id = 2, - }, - .controller_param_init_config = { - .angle_PID = { - .Kp = 10, // 10 - .Ki = 0, - .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - .MaxOut = 500, + .can_init_config = { + .can_handle = &hcan2, + .tx_id = 4, }, - .speed_PID = { - .Kp = 50, // 50 - .Ki = 350, // 350 - .Kd = 0, // 0 - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 2500, - .MaxOut = 20000, + .controller_param_init_config = { + .angle_PID = { + .Kp = 7.5, // 10 + .Ki = 0, + .Kd = 0.05, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 100, + .MaxOut = 500, + }, + .speed_PID = { + .Kp = -300, // 50 + .Ki = 0, // 350 + .Kd = 0, // 0 + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 2500, + .MaxOut = 20000, + }, + .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, + // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 + .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[0], }, - .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, - // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 - .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), - }, - .controller_setting_init_config = { - .angle_feedback_source = OTHER_FEED, - .speed_feedback_source = OTHER_FEED, - .outer_loop_type = ANGLE_LOOP, - .close_loop_type = SPEED_LOOP | ANGLE_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - }, - .motor_type = GM6020, + .controller_setting_init_config = { + .angle_feedback_source = OTHER_FEED, + .speed_feedback_source = OTHER_FEED, + .outer_loop_type = ANGLE_LOOP, + .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + }, + .motor_type = GM6020, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); @@ -100,55 +101,66 @@ void GimbalInit() } /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ -void GimbalTask() -{ +void GimbalTask() { // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - switch (gimbal_cmd_recv.gimbal_mode) - { - // 停止 - case GIMBAL_ZERO_FORCE: - DJIMotorStop(yaw_motor); - DJIMotorStop(pitch_motor); - break; - // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 - case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) - DJIMotorEnable(yaw_motor); - DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); - DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); - DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 - DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - break; - default: - break; + switch (gimbal_cmd_recv.gimbal_mode) { + // 停止 + case GIMBAL_ZERO_FORCE: + DJIMotorStop(yaw_motor); + DJIMotorStop(pitch_motor); + break; + // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 + case GIMBAL_GYRO_MODE: // 后续只保留此模式 + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + break; + // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 + case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); + DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); + DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); + DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 + DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + break; + default: + break; } // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // ... + //float vofa_send_data[4]; +// vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref; +// vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure; +// vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref; +// vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure; +// +// vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref; +// vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure; +// vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref; +// vofa_send_data[3] = yaw_motor->motor_controller.angle_PID.Measure; +// vofa_justfloat_output(vofa_send_data, 16, &huart1); + + // 设置反馈数据,主要是imu和yaw的ecd gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; // 推送消息 - PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); + PubPushMessage(gimbal_pub, (void *) &gimbal_feedback_data); } \ No newline at end of file diff --git a/application/robot.c b/application/robot.c index 3c3b765..7b4c41e 100644 --- a/application/robot.c +++ b/application/robot.c @@ -31,7 +31,7 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); -// GimbalInit(); + GimbalInit(); // ShootInit(); #endif @@ -49,7 +49,7 @@ void RobotTask() { #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDTask(); - //GimbalTask(); + GimbalTask(); //ShootTask(); #endif diff --git a/application/robot_def.h b/application/robot_def.h index 840b171..58f2280 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -26,11 +26,11 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 2711 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_ALIGN_ECD 1995 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 -#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 -#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) -#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 +#define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f diff --git a/bsp/usb/bsp_usb.c b/bsp/usb/bsp_usb.c index 9c4ad2f..045fcb5 100644 --- a/bsp/usb/bsp_usb.c +++ b/bsp/usb/bsp_usb.c @@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf) void USBTransmit(uint8_t *buffer, uint16_t len) { - CDC_Transmit_FS(buffer, len); // 发送 + //CDC_Transmit_FS(buffer, len); // 发送 } diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c index 9ae72d6..4ea4ae8 100644 --- a/modules/vofa/vofa.c +++ b/modules/vofa/vofa.c @@ -30,6 +30,6 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart send_data[4 * num + 2] = 0x80; send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 - HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); - //CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); + //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100); + CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); }