机械臂达妙电机can通信问题 需要顺序初始化
This commit is contained in:
parent
2f654113c6
commit
c8bb8154f9
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||||
.Improve = static_cast<PID_Improvement_e>(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_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||||
.Improve = static_cast<PID_Improvement_e>( 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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
@ -45,3 +46,21 @@ 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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -211,7 +211,8 @@ void DMMotorTask(void const *argument)
|
|||
|
||||
CANTransmit(motor->motor_can_instance, 1);
|
||||
|
||||
osDelay(2);
|
||||
//osDelay(2);
|
||||
osDelay(5);
|
||||
}
|
||||
}
|
||||
void DMMotorControlInit()
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue