TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

装超电后为预检录准备的版本

Browse Source
This commit is contained in:
chenfu
2024-05-26 11:40:33 +08:00
parent 346a0f4d70
commit da591dcfb4
8 changed files with 83 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
application/cmd/robot_cmd.c
View File

@@ -83,11 +83,11 @@ static void CalcOffsetAngle()
// @todo:相差一整圈时会出问题，待修复
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
uint16_t yaw_chassis_align_ecd;
if(chassis_direction == CHASSIS_ALIGN)
if (chassis_direction == CHASSIS_ALIGN)
{
yaw_chassis_align_ecd = 2716;
}
else if(chassis_direction == CHASSIS_SIDLE)
else if (chassis_direction == CHASSIS_SIDLE)
{
yaw_chassis_align_ecd = 765;
}
@@ -96,7 +96,7 @@ static void CalcOffsetAngle()
yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
if (yaw_chassis_align_ecd > 4096) // 如果大于180度
if (yaw_chassis_align_ecd > 4096) // 如果大于180度
{
if (angle > yaw_align_angle)
chassis_cmd_send.offset_angle = angle - yaw_align_angle;
@@ -106,7 +106,7 @@ static void CalcOffsetAngle()
chassis_cmd_send.offset_angle = angle - yaw_align_angle + 360.0f;
}
else
{ // 小于180度
{ // 小于180度
if (angle > yaw_align_angle && angle <= 180.0f + yaw_align_angle)
chassis_cmd_send.offset_angle = angle - yaw_align_angle;
else if (angle > 180.0f + yaw_align_angle)
@@ -122,19 +122,21 @@ static void CalcOffsetAngle()
*/
static void RemoteControlSet()
{
memcpy(&rc_data->key_count, 0, sizeof(rc_data->key_count));
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
// 云台参数,确定云台控制数据
gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_;
gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1;
chassis_cmd_send.rotate_w = 0.0f;
// 摇杆控制的软件限位
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[下],视觉模式
{
gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw );
gimbal_cmd_send.pitch = ( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch );
gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
}
// 底盘参数
@@ -150,7 +152,17 @@ static void RemoteControlSet()
// 右侧开关状态为[下],底盘旋转
if (switch_is_down(rc_data[TEMP].rc.switch_right))
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 4.0f;
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
if (abs(rc_data[TEMP].rc.rocker_r_) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
}
if (abs(rc_data[TEMP].rc.rocker_r1) > 300)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE;
}
chassis_cmd_send.vx = 0;
}
else
@@ -182,7 +194,7 @@ static void RemoteControlSet()
// 发射参数
shoot_cmd_send.bullet_speed = 30;
shoot_cmd_send.shoot_rate = 15;
shoot_cmd_send.shoot_rate = 10;
}
/**
@@ -193,15 +205,14 @@ static void MouseKeySet()
{
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f ; // 系数待测
gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f; // 系数待测
gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f;
gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE);
if(chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
if (chassis_cmd_send.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
{
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s
- rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d);
}
else if (chassis_cmd_send.chassis_mode == CHASSIS_ROTATE)
{
@@ -218,17 +229,14 @@ static void MouseKeySet()
{
chassis_direction = CHASSIS_SIDLE;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2)
{
case 0:
break;
case 1:
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
break;
default:
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮
{
case 0:
@@ -239,7 +247,7 @@ static void MouseKeySet()
break;
}
if(shoot_cmd_send.friction_mode == FRICTION_ON)
if (shoot_cmd_send.friction_mode == FRICTION_ON)
{
if (rc_data[TEMP].mouse.press_l)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
@@ -249,7 +257,7 @@ static void MouseKeySet()
else
shoot_cmd_send.load_mode = LOAD_STOP;
switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键刷新UI
switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键刷新UI
{
case 0:
chassis_cmd_send.ui_mode = UI_KEEP;
@@ -264,6 +272,15 @@ static void MouseKeySet()
gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw);
gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch);
}
if (rc_data[TEMP].key[KEY_PRESS].q)
{
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
chassis_cmd_send.rotate_w = 6.0f;
}
if (rc_data[TEMP].key[KEY_PRESS].r)
{
chassis_cmd_send.chassis_mode = CHASSIS_RESET;
}
}
/**
@@ -302,9 +319,7 @@ static void EmergencyHandler()
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C] % 2) // ctrl+c 进入急停
{
case 0:
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
break;
default: