From c8bb8154f9e91ee09ae9569a57c4238b9fb25be2 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Mon, 13 May 2024 23:58:29 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E8=BE=BE=E5=A6=99?= =?UTF-8?q?=E7=94=B5=E6=9C=BAcan=E9=80=9A=E4=BF=A1=E9=97=AE=E9=A2=98=20?= =?UTF-8?q?=E9=9C=80=E8=A6=81=E9=A1=BA=E5=BA=8F=E5=88=9D=E5=A7=8B=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- Src/main.c | 4 + application/cmd/robot_cmd.c | 52 +++++-- application/gimbal/gimbal.cpp | 51 ++++--- application/robot_def.h | 13 ++ application/to_stretch/to_stretch.c | 19 ++- engineering.jdebug.user | 61 ++++++--- modules/motor/Air_pump/air_pump.c | 4 +- modules/motor/DMmotor/dmmotor.c | 3 +- modules/motor/motor_task.c | 2 +- modules/referee/referee_task.c | 204 +++++++++++++++------------- modules/referee/rm_referee.h | 7 + 12 files changed, 267 insertions(+), 155 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d553140..71674bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D modules modules/alarm modules/algorithm modules/auto_aim modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/robotics - modules/motor/DJImotor modules/motor/Dmmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor + modules/motor/DJImotor modules/motor/Dmmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/Air_pump application application/chassis application/cmd application/gimbal application/to_stretch Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) diff --git a/Src/main.c b/Src/main.c index b36416f..0e68f95 100644 --- a/Src/main.c +++ b/Src/main.c @@ -121,6 +121,10 @@ int main(void) MX_DAC_Init(); /* USER CODE BEGIN 2 */ + // 气泵上电默认关闭 + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,20000); + HAL_Delay(1500); //等待达妙电机上电初始化 RobotInit(); // 唯一的初始化函数 LOGINFO("[main] SystemInit() and RobotInit() done"); diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 72311c5..9847a76 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -11,6 +11,8 @@ #include "auto_aim.h" #include "super_cap.h" #include "user_lib.h" + +#include "air_pump.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" @@ -40,6 +42,8 @@ static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回 static SendPacket_t vision_send_data; // 视觉发送数据 +PumpInstance *air_pump; //吸盘 + //自瞄相关信息 static Trajectory_Type_t trajectory_cal; static Aim_Select_Type_t aim_select; @@ -69,6 +73,8 @@ void RobotCMDInit() { referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI referee_VT_data = VTInit(&huart1); + air_pump = PumpInit(&htim1,TIM_CHANNEL_1); + gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); to_stretch_cmd_pub = PubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s)); @@ -133,13 +139,32 @@ static void death_check() * */ static void update_ui_data() { + ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode; + ui_data.chassis_mode = chassis_cmd_send.chassis_mode; + ui_data.to_stretch_mode = to_stretch_cmd_send.to_stretch_mode; + ui_data.xyz[1] = to_stretch_fetch_data.protract_x; + ui_data.xyz[2] = to_stretch_fetch_data.lift_z; } // 出招表 static void RemoteControlSet() { + // 吸盘气泵控制,拨轮向上打为负,向下为正 + if (rc_data[TEMP].rc.dial < -300) // 向上超过300,打开气泵 + { + Pump_open(air_pump); + ui_data.pump_mode = PUMP_OPEN; + } + else + { + Pump_stop(air_pump); + ui_data.pump_mode = PUMP_STOP; + } + + + if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动 { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; @@ -223,9 +248,12 @@ static void RemoteControlSet() { static void MouseKeySet() { chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测 chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000; + chassis_cmd_send.wz = (float) rc_data[TEMP].mouse.x / 660 * 4; - to_stretch_cmd_send.ud -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测 - to_stretch_cmd_send.fb -= (float) rc_data[TEMP].mouse.y / 660 * 6; + to_stretch_cmd_send.tc -= (float) rc_data[TEMP].mouse.y / 660 * 6; + + to_stretch_cmd_send.ud += rc_data[TEMP].key[KEY_PRESS].q * 10 - rc_data[TEMP].key[KEY_PRESS].e * 10; + to_stretch_cmd_send.fb += rc_data[TEMP].key[KEY_PRESS].z * 10 - rc_data[TEMP].key[KEY_PRESS].c * 10; switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI { @@ -236,15 +264,17 @@ static void MouseKeySet() { default: break; } -// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V键开关红点激光 -// { -// case 0: -// HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_SET); -// break; -// default: -// HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_RESET); -// break; -// } + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F按键开关气泵 + { + case 0: + Pump_stop(air_pump); + ui_data.pump_mode = PUMP_STOP; + break; + default: + Pump_open(air_pump); + ui_data.pump_mode = PUMP_OPEN; + break; + } // switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 // { // case 0: diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index 4b28fe5..b9f3652 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -83,11 +83,11 @@ void GimbalInit() .controller_param_init_config = { .other_speed_feedback_ptr = &yaw_spd, .speed_PID = { - .Kp = 3,//5, - .Ki = 3.0f, + .Kp = 3.5f,//5, + .Ki = 0.0f, .Kd = 0.02f, .MaxOut = 10, - .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .Improve = static_cast(PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, }, .angle_PID = { @@ -135,9 +135,7 @@ void GimbalInit() pitch_motor_config.can_init_config.tx_id = 0x03; pitch_motor_config.can_init_config.rx_id = 0x04; pitch_motor_l = DMMotorInit(&pitch_motor_config); - pitch_motor_config.can_init_config.tx_id = 0x13; - pitch_motor_config.can_init_config.rx_id = 0x14; - pitch_motor_r = DMMotorInit(&pitch_motor_config); + PID_Init_Config_s pitch_spd_config= { .Kp = 2.5f, .Ki = 0.8f, @@ -196,12 +194,13 @@ void GimbalInit() Motor_Init_Config_s diff_motor_config = { .controller_param_init_config = { .speed_PID = { - .Kp = 0.6f, - .Ki = 0.1f, - .Kd = 0.02f, + .Kp = 0.2f,//0.6f, + .Ki = 0,//0.1f, + .Kd = 0.02,//0.02f, .MaxOut = 10, - .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .Improve = static_cast( PID_Integral_Limit | PID_Derivative_On_Measurement), .IntegralLimit = 10.0F, + .Output_LPF_RC = 0.02F, }, }, .controller_setting_init_config = { @@ -228,7 +227,7 @@ void GimbalInit() const float spd_filter_num = 0.05f; first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num); - const float spd_filter_num_yaw = 0.1f; + const float spd_filter_num_yaw = 0.05f; first_order_filter_init(&yaw_spd_filter,5e-3,&spd_filter_num_yaw); const float spd_filter_num_roll = 0.05f; first_order_filter_init(&roll_spd_filter,5e-3,&spd_filter_num_roll); @@ -238,7 +237,7 @@ void GimbalInit() first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff); PID_Init_Config_s diff_pitch_config= { - .Kp = 15.0f, + .Kp = 20.0F,//15.0f, .Ki = 0, .Kd = 0, .MaxOut = 10, @@ -277,6 +276,10 @@ void GimbalInit() }; PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config); + pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！ + pitch_motor_config.can_init_config.rx_id = 12; + pitch_motor_r = DMMotorInit(&pitch_motor_config); + gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -297,9 +300,17 @@ void GimbalInit() //} /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ + +//float debug_diff_kp = 0.1f,debug_diff_ki=0,debug_diff_kd = 0; void GimbalTask() { - +// diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp; +// diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki; +// diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd; +// +// diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp; +// diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki; +// diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd; // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); @@ -401,17 +412,17 @@ void GimbalTask() err[i] = (ik - arm_q3).norm(); } - if (err[1] >= err[0]) +// if (err[1] >= err[0]) +// ik_q3_cmd = ik_q3.block<3,1>(0,0); +// else +// ik_q3_cmd = ik_q3.block<3,1>(0,1); + +// //选用 roll误差小的一组解 + if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0])) ik_q3_cmd = ik_q3.block<3,1>(0,0); else ik_q3_cmd = ik_q3.block<3,1>(0,1); -// //选用 roll角度小的一组解 -// if (abs(ik_q3[0][0]) <= abs(ik_q3[0][1])) -// -// else -// ik_q3_cmd = ik_q3.block<3,1>(0,1); - ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD); //ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD); diff --git a/application/robot_def.h b/application/robot_def.h index 74a5f2c..211637e 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -35,6 +35,13 @@ #define YAW 60 +//龙门架限位 +#define UD_MAX 100 +#define UD_MIN 0 + +#define FB_MIN 0 +#define FB_MAX 100 + // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 @@ -114,6 +121,12 @@ typedef enum ADJUST_PITCH_MODE, //找pitch的限位 } gimbal_mode_e; +//气泵模式 +typedef enum +{ + PUMP_STOP, //关气泵 + PUMP_OPEN //开气泵 +} pump_mode_e; // 伸缩模式设置 typedef enum diff --git a/application/to_stretch/to_stretch.c b/application/to_stretch/to_stretch.c index 4cd45f7..136b4d3 100644 --- a/application/to_stretch/to_stretch.c +++ b/application/to_stretch/to_stretch.c @@ -18,14 +18,14 @@ static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反 static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan; -float gravity_feedforward = 3000; +float gravity_feedforward = 4000; float l_protract = 0,r_protract = 0; float protract_x = 0; //前伸距离 float l_lift = 0,r_lift = 0; float lift_z = 0; //抬升高度 float ld_offset = 0,rd_offset = 0; float lu_offset = 0,ru_offset = 0; -float kp=4,ki=1,kd=0;//调试用 +float kp=3,ki=2,kd=0;//调试用 PIDInstance protract_position_loop;//前伸位置环 PIDInstance lift_position_loop;//抬升位置环 @@ -46,11 +46,11 @@ void To_stretchInit() { .MaxOut = 500, }, .speed_PID = { - .Kp = 4, - .Ki = 1, + .Kp = 3, + .Ki = 2, .Kd = 0, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 2500, + .IntegralLimit = 5000, .MaxOut = 13000, }, .current_feedforward_ptr = &gravity_feedforward, @@ -140,7 +140,7 @@ void To_stretchInit() { // 图传电机 Motor_Init_Config_s tuchuan_config = { .can_init_config = { - .can_handle = &hcan1, + .can_handle = &hcan2, .tx_id = 1, }, .controller_param_init_config = { @@ -182,6 +182,10 @@ void To_stretchTask() motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki; motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd; + motor_ru->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp; + motor_ru->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki; + motor_ru->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd; + static uint8_t init_flag = FALSE; if(init_flag == FALSE) { @@ -258,6 +262,9 @@ void To_stretchTask() } // 推送反馈消息 + + to_stretch_feedback_data.protract_x = protract_x; + to_stretch_feedback_data.lift_z = lift_z; #ifdef ONE_BOARD PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data); #endif diff --git a/engineering.jdebug.user b/engineering.jdebug.user index 9bb76cc..247246d 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,36 +1,37 @@ -Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:276:5, State=BP_STATE_DISABLED -GraphedExpression="(gimbal_cmd_recv).pitch", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f -GraphedExpression="(referee_VT_data)->CmdID", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b -OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59 +Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED +OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52 +OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=221 +OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.c", Line=9 +OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=70 +OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=36 +OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141 OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.h", Line=0 OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107 -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 -OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=255 -OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3637 +OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=213 +OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=9 +OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29 -OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51 -OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=48 -OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315 +OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59 +OpenDocument="robot_task.h", FilePath="D:/CLion/Project/engineering/application/robot_task.h", Line=109 +OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3641 OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255 -OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0 +OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=355 OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412 OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=204, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=531, h=191, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=401, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=437, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1041, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=265, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=531, h=450, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="312;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0" -OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=210, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=367, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=687, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=979, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="710;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="724;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=252, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (gimbal_cmd_recv).pitch";" (referee_VT_data)->CmdID"], ColWidths=[100;100;100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;126;100;100;100;100;100;107;107] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[341;100;100;100;100;100;100;107;478] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;100;100] @@ -44,4 +45,22 @@ WatchedExpression="cmd_xyz.data_", RefreshRate=2, Window=Watched Data 1 WatchedExpression="T_cmd", Window=Watched Data 1 WatchedExpression="T_cmd.data_", RefreshRate=2, Window=Watched Data 1 WatchedExpression="referee_VT_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="ik_q3.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="ik_q3_cmd.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="fk_T.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="cmd_R.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="gimbal_cmd_recv", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="CustomControl", Window=Watched Data 1 +WatchedExpression="air_pump", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="motor_lu", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="motor_ru", Window=Watched Data 1 +WatchedExpression="(((motor_lu)->motor_controller).speed_PID).Ref", Window=Watched Data 1 +WatchedExpression="kp", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="ki", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1 +WatchedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Window=Watched Data 1 +WatchedExpression="yaw_spd_filter", Window=Watched Data 1 WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file diff --git a/modules/motor/Air_pump/air_pump.c b/modules/motor/Air_pump/air_pump.c index dc46049..6066817 100644 --- a/modules/motor/Air_pump/air_pump.c +++ b/modules/motor/Air_pump/air_pump.c @@ -22,10 +22,10 @@ PumpInstance *PumpInit(TIM_HandleTypeDef *htim,uint32_t Channel) { } void Pump_open(PumpInstance *pump) { - __HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,20000); + __HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,0); } void Pump_stop(PumpInstance *pump) { - __HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,0); + __HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,20000); } \ No newline at end of file diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index 0915e60..2d1086b 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -211,7 +211,8 @@ void DMMotorTask(void const *argument) CANTransmit(motor->motor_can_instance, 1); - osDelay(2); + //osDelay(2); + osDelay(5); } } void DMMotorControlInit() diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 17c281e..f2eac6e 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -10,7 +10,7 @@ void MotorControlTask() // static uint8_t cnt = 0; 设定不同电机的任务频率 // if(cnt%5==0) //200hz // if(cnt%10==0) //100hz - DJIMotorControl(); + //DJIMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ //LKMotorControl(); diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 7371f61..0c790cc 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -42,6 +42,7 @@ static void DeterminRobotID() static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data); static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测 static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化 +static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data); referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data) { @@ -53,78 +54,59 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int void UITask() { - //RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常 + RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常 MyUIRefresh(referee_recv_info, Interactive_data); + xyz_refresh(referee_recv_info, Interactive_data); } -static Graph_Data_t UI_shoot_line[10]; // 射击准线 -static Graph_Data_t UI_shoot_yuan[2]; // 射击圆心 -static Graph_Data_t UI_Energy[4]; // 电容能量条 static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次 static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change -static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565}; +static Graph_Data_t UI_State_dyn_graph[2]; + +#define AXIS_LEN 250 +#define AXIS_X0 550 +#define AXIS_Y0 80 +static Graph_Data_t xyz_axis[6]; // 抬升 横移位置 void MyUIInit() { - if (!referee_recv_info->init_flag) - vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务 + //if (!referee_recv_info->init_flag) + //vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务 while (referee_recv_info->GameRobotState.robot_id == 0) osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查 DeterminRobotID(); // 确定ui要发送到的目标客户端 UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI - // 绘制发射基准线和基准圆 - UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]); - UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740); - UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]); - UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Orange, 2, 960, 200, 960, 600); - UICircleDraw(&UI_shoot_yuan[0],"sy0",UI_Graph_ADD,7,UI_Color_Yellow,2,960,540,10); - UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3],UI_shoot_yuan[0]); - // 绘制车辆状态标志指示 - UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:"); + UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "F.AIR_PUMP:"); + UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]); - UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]); - UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]); - UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "F.frict:"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]); - UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "Q.lid:"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]); + // xyz坐标系 静态 + UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0); + UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN); + UILineDraw(&xyz_axis[2], "z_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN); + UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[0]); + UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[1],xyz_axis[2]); + + // xyz坐标系 动态 + UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0); + UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0+AXIS_LEN*0.5); + UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN*0.5); + UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[3]); + UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[4],xyz_axis[5]); + // 绘制车辆状态标志，动态 // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新 - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); - UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]); - UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]); - UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open "); - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]); - // 底盘功率显示，静态 - UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:"); - UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]); + //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743); + UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 720, 10); - // 能量条框 - UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180); - UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]); + UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]); +// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); - // 底盘功率显示,动态 - UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000); - UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]); - - // 能量条初始状态 - UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160); - - //超级电容电压 -// UIIntDraw(&UI_Energy[3],"sd7",UI_Graph_ADD,8,UI_Color_Pink,18,2,1000,300,cap->cap_msg.cap_vol); -// UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]); } // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常 @@ -172,50 +154,67 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i { UIChangeCheck(_Interactive_data); // chassis - if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1) +// if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1) +// { +// switch (_Interactive_data->chassis_mode) +// { +// case CHASSIS_ZERO_FORCE: +// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); +// break; +// case CHASSIS_ROTATE: +// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate "); +// // 此处注意字数对齐问题，字数相同才能覆盖掉 +// break; +// case CHASSIS_NO_FOLLOW: +// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow "); +// break; +// case CHASSIS_FOLLOW_GIMBAL_YAW: +// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); +// break; +// } +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); +// _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; +// } +// // gimbal +// if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) +// { +// switch (_Interactive_data->gimbal_mode) +// { +// case GIMBAL_ZERO_FORCE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); +// break; +// } +// case GIMBAL_FREE_MODE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); +// break; +// } +// case GIMBAL_GYRO_MODE: +// { +// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); +// break; +// } +// } +// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); +// _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0; +// } + // pump + if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1) { - switch (_Interactive_data->chassis_mode) - { - case CHASSIS_ZERO_FORCE: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); - break; - case CHASSIS_ROTATE: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate "); - // 此处注意字数对齐问题，字数相同才能覆盖掉 - break; - case CHASSIS_NO_FOLLOW: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow "); - break; - case CHASSIS_FOLLOW_GIMBAL_YAW: - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); - break; + switch (_Interactive_data->pump_mode) { + case PUMP_STOP: + //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743); + UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 720, 10); + break; + case PUMP_OPEN: + //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695); + UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 720, 25); + break; } - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); - _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; - } - // gimbal - if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) - { - switch (_Interactive_data->gimbal_mode) - { - case GIMBAL_ZERO_FORCE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); - break; - } - case GIMBAL_FREE_MODE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); - break; - } - case GIMBAL_GYRO_MODE: - { - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); - break; - } - } - UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); - _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0; + UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]); + + _Interactive_data->Referee_Interactive_Flag.pump_flag = 0; } } @@ -238,4 +237,25 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data) _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1; _Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode; } + + if (_Interactive_data->pump_mode != _Interactive_data->pump_last_mode) + { + _Interactive_data->Referee_Interactive_Flag.pump_flag = 1; + _Interactive_data->pump_last_mode = _Interactive_data->pump_mode; + } +} + +static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data) +{ + const float y_max = 100;//单位mm + const float z_max = 100;//单位mm + + float y_ratio = _Interactive_data->xyz[1]/y_max; + float z_ratio = _Interactive_data->xyz[2]/z_max; + // xyz坐标系 动态 + UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0); + UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio)); + UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio)); + UIGraphRefresh(&referee_recv_info->referee_id,3,xyz_axis[3],xyz_axis[4],xyz_axis[5]); + } diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index 47ccdea..a853152 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -73,6 +73,8 @@ typedef struct uint32_t Power_flag : 1; uint32_t aim_flag : 1; uint32_t cap_flag : 1; + + uint32_t pump_flag : 1; } Referee_Interactive_Flag_t; // 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据 @@ -82,12 +84,17 @@ typedef struct // 为UI绘制以及交互数据所用 chassis_mode_e chassis_mode; // 底盘模式 gimbal_mode_e gimbal_mode; // 云台模式 + pump_mode_e pump_mode; // 气泵模式 + to_stretch_mode_e to_stretch_mode; // 抬升+前伸模式 // 上一次的模式，用于flag判断 chassis_mode_e chassis_last_mode; gimbal_mode_e gimbal_last_mode; + pump_mode_e pump_last_mode; + to_stretch_mode_e to_stretch_last_mode; uint8_t aim_last_fire; + float xyz[3]; // xyz三轴位置 } Referee_Interactive_info_t;