diff --git a/.project b/.project index c062ef3..0849703 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - Swing_Rust_UDP_cabled + Welding_Rust_UDP_cabled diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index d93be1c..c57f8c6 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/Core/BASE/Inc/BSP/B03_Para_01_100.h b/Core/BASE/Inc/BSP/B03_Para_01_100.h index 2e51163..106b559 100644 --- a/Core/BASE/Inc/BSP/B03_Para_01_100.h +++ b/Core/BASE/Inc/BSP/B03_Para_01_100.h @@ -13,7 +13,7 @@ #include "bsp_PID.pb.h" //int32_t 定义头文件 #include -#define ROBOT_NUMBER 3 +#define ROBOT_NUMBER 4 // //typedef struct { // int32_t Speed_m_per_min_1 ; // MPMin = 1m/min 1米每分钟 m/min @@ -58,11 +58,6 @@ typedef struct { - - - - - // 声明100组DH参数数组（extern关键） extern B03_Para_t g_B03_param_table[100]; #endif /* BASE_INC_B03_PARA_01_100_H_ */ diff --git a/Core/BASE/Inc/BSP/BHBF_ROBOT.h b/Core/BASE/Inc/BSP/BHBF_ROBOT.h index 322f2be..39652a5 100644 --- a/Core/BASE/Inc/BSP/BHBF_ROBOT.h +++ b/Core/BASE/Inc/BSP/BHBF_ROBOT.h @@ -54,6 +54,7 @@ #include "change_line_control.h" #include "fsm_state.h" #include "robot_move_actions.h" +#include "msp_TTMotor_ZQ.h" //#include "robot_state.h" //FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 896K @@ -73,6 +74,7 @@ extern CV_struct_define CV; extern PV_struct_define decoded_PV; extern PV_struct_define decoded_PV_variable; extern PV_struct_define _decoded_PV_temp; +extern int robot_version; typedef struct sys_timer_handler { diff --git a/Core/BASE/Inc/BSP/bsp_devic_moniter.h b/Core/BASE/Inc/BSP/bsp_devic_moniter.h index 4a2d4a7..7f7777b 100644 --- a/Core/BASE/Inc/BSP/bsp_devic_moniter.h +++ b/Core/BASE/Inc/BSP/bsp_devic_moniter.h @@ -19,10 +19,17 @@ extern "C" { * 设备ID *==========================*/ typedef enum { - //DEV_GYRO = 0, - //DEV_SBUS, - DEV_LEFT_MOTOR=0, - //DEV_RIGHT_MOTOR, + DEV_SBUS =0, + DEV_Serial, + DEV_GYRO, + DEV_LEFT_MOTOR, + DEV_RIGHT_MOTOR, + DEV_SWING_MOTOR, + DEV_SENSOR, + DEV_ULTRA, + DEV_YOUXIAN, + DEV_TUIGAN, + DEV_DROUND, DEV_COUNT // 总设备数 } DeviceId; diff --git a/Core/BASE/Protobuf/PSource/bsp_Error.pb.h b/Core/BASE/Protobuf/PSource/bsp_Error.pb.h index bf73b79..ca529a7 100644 --- a/Core/BASE/Protobuf/PSource/bsp_Error.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_Error.pb.h @@ -17,12 +17,11 @@ typedef enum _ComError { ComError_TL720D = 3, ComError_ZQ_CAN_ID1_LeftMotor = 4, ComError_ZQ_CAN_ID2_RightMotor = 5, - ComError_ZQ_CAN_ID3_SwingMotor = 6, - ComError_Force_Sensor = 7, - ComError_Ultrasonic_Sensor = 8, - ComError_Android_485 = 9, /* UWB_20_Error=10; */ - ComError_Strain_Gauge = 10, - ComError_Ground_Management = 11 + ComError_Force_Sensor = 6, + ComError_Ultrasonic_Sensor = 7, + ComError_Android_485 = 8, /* UWB_20_Error=10; */ + ComError_Strain_Gauge = 9, + ComError_Ground_Management = 10 } ComError; /* Struct definitions */ diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index 60d2f7a..0d92efe 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -65,6 +65,7 @@ typedef struct _GV_struct_define { int32_t client_close; int32_t robot_real_speed; int32_t wire_status; + float robot_back_speed; /* 边打边退模式下机器人的后退速度 */ } GV_struct_define; @@ -73,8 +74,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define GV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, MSP_TL720DParameters_init_default, false, IO_Data_init_default, false, ErrorData_init_default, false, PV_struct_define_init_default, 0, 0, 0, 0, 0, false, Strain_Gauge_Struct_init_default, 0, 0, 0, 0, false, ground_management_struct_init_default, 0, 0, 0, 0, 0} -#define GV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, MSP_TL720DParameters_init_zero, false, IO_Data_init_zero, false, ErrorData_init_zero, false, PV_struct_define_init_zero, 0, 0, 0, 0, 0, false, Strain_Gauge_Struct_init_zero, 0, 0, 0, 0, false, ground_management_struct_init_zero, 0, 0, 0, 0, 0} +#define GV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, MSP_TL720DParameters_init_default, false, IO_Data_init_default, false, ErrorData_init_default, false, PV_struct_define_init_default, 0, 0, 0, 0, 0, false, Strain_Gauge_Struct_init_default, 0, 0, 0, 0, false, ground_management_struct_init_default, 0, 0, 0, 0, 0, 0} +#define GV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, MSP_TL720DParameters_init_zero, false, IO_Data_init_zero, false, ErrorData_init_zero, false, PV_struct_define_init_zero, 0, 0, 0, 0, 0, false, Strain_Gauge_Struct_init_zero, 0, 0, 0, 0, false, ground_management_struct_init_zero, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define GV_struct_define_TempatureE_2C_tag 1 @@ -112,6 +113,7 @@ extern "C" { #define GV_struct_define_client_close_tag 33 #define GV_struct_define_robot_real_speed_tag 34 #define GV_struct_define_wire_status_tag 35 +#define GV_struct_define_robot_back_speed_tag 36 /* Struct field encoding specification for nanopb */ #define GV_struct_define_FIELDLIST(X, a) \ @@ -149,7 +151,8 @@ X(a, STATIC, SINGULAR, INT32, robot_back_distance, 31) \ X(a, STATIC, SINGULAR, INT32, auto_working, 32) \ X(a, STATIC, SINGULAR, INT32, client_close, 33) \ X(a, STATIC, SINGULAR, INT32, robot_real_speed, 34) \ -X(a, STATIC, SINGULAR, INT32, wire_status, 35) +X(a, STATIC, SINGULAR, INT32, wire_status, 35) \ +X(a, STATIC, SINGULAR, FLOAT, robot_back_speed, 36) #define GV_struct_define_CALLBACK NULL #define GV_struct_define_DEFAULT NULL #define GV_struct_define_P_MK32_MSGTYPE SP_MSP_MK32_Button @@ -170,7 +173,7 @@ extern const pb_msgdesc_t GV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size -#define GV_struct_define_size 2086 +#define GV_struct_define_size 2092 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/PSource/bsp_IV.pb.h b/Core/BASE/Protobuf/PSource/bsp_IV.pb.h index 48ba781..ebda3d7 100644 --- a/Core/BASE/Protobuf/PSource/bsp_IV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_IV.pb.h @@ -44,10 +44,6 @@ typedef struct _IV_struct_define { 取值规则：0 = 正常（无报警）；1 = 报警（过流/过载/堵转等） 作用：上报右轮驱动电机的故障状态，用于电机故障排查 */ int32_t Right_Motor_Err; - /* 摆臂电机报警状态 - 取值规则：0 = 正常（无报警）；1 = 报警（过流/过载/堵转等） - 作用：上报抛丸/喷砂摆臂电机的故障状态，适配摆臂速度≥25°/S的作业要求 */ - int32_t Swing_Motor_Err; /* 机器人在线状态 取值规则：0 = 离线（通讯中断）；1 = 在线（通讯正常） 作用：APP/上位机判断机器人通讯状态，离线时触发声光报警 */ @@ -81,6 +77,7 @@ typedef struct _IV_struct_define { int32_t reason_of_robot_error; /* 获取有线连接还是无线连接 */ int32_t wire_or_wireless; + int32_t Robot_version; } IV_struct_define; @@ -101,22 +98,22 @@ extern "C" { #define IV_struct_define_SystemError_tag 6 #define IV_struct_define_Left_Motor_Err_tag 7 #define IV_struct_define_Right_Motor_Err_tag 8 -#define IV_struct_define_Swing_Motor_Err_tag 9 -#define IV_struct_define_Is_Online_tag 10 -#define IV_struct_define_Spara_Data_1_tag 11 -#define IV_struct_define_Spara_Data_2_tag 12 -#define IV_struct_define_Spara_Data_3_tag 13 -#define IV_struct_define_Weld_data_tag 14 -#define IV_struct_define_Weld_exist_tag 15 -#define IV_struct_define_Turn_difference_tag 16 -#define IV_struct_define_Present_press_tag 17 -#define IV_struct_define_left_angle_tag 18 -#define IV_struct_define_right_angle_tag 19 -#define IV_struct_define_robot_start_tag 20 -#define IV_struct_define_robot_set_speed_tag 21 -#define IV_struct_define_auto_mode_status_tag 22 -#define IV_struct_define_reason_of_robot_error_tag 23 -#define IV_struct_define_wire_or_wireless_tag 24 +#define IV_struct_define_Is_Online_tag 9 +#define IV_struct_define_Spara_Data_1_tag 10 +#define IV_struct_define_Spara_Data_2_tag 11 +#define IV_struct_define_Spara_Data_3_tag 12 +#define IV_struct_define_Weld_data_tag 13 +#define IV_struct_define_Weld_exist_tag 14 +#define IV_struct_define_Turn_difference_tag 15 +#define IV_struct_define_Present_press_tag 16 +#define IV_struct_define_left_angle_tag 17 +#define IV_struct_define_right_angle_tag 18 +#define IV_struct_define_robot_start_tag 19 +#define IV_struct_define_robot_set_speed_tag 20 +#define IV_struct_define_auto_mode_status_tag 21 +#define IV_struct_define_reason_of_robot_error_tag 22 +#define IV_struct_define_wire_or_wireless_tag 23 +#define IV_struct_define_Robot_version_tag 24 /* Struct field encoding specification for nanopb */ #define IV_struct_define_FIELDLIST(X, a) \ @@ -128,22 +125,22 @@ X(a, STATIC, SINGULAR, INT32, Distance_Sensor, 5) \ X(a, STATIC, SINGULAR, INT32, SystemError, 6) \ X(a, STATIC, SINGULAR, INT32, Left_Motor_Err, 7) \ X(a, STATIC, SINGULAR, INT32, Right_Motor_Err, 8) \ -X(a, STATIC, SINGULAR, INT32, Swing_Motor_Err, 9) \ -X(a, STATIC, SINGULAR, INT32, Is_Online, 10) \ -X(a, STATIC, SINGULAR, INT32, Spara_Data_1, 11) \ -X(a, STATIC, SINGULAR, INT32, Spara_Data_2, 12) \ -X(a, STATIC, SINGULAR, INT32, Spara_Data_3, 13) \ -X(a, STATIC, SINGULAR, INT32, Weld_data, 14) \ -X(a, STATIC, SINGULAR, INT32, Weld_exist, 15) \ -X(a, STATIC, SINGULAR, INT32, Turn_difference, 16) \ -X(a, STATIC, SINGULAR, INT32, Present_press, 17) \ -X(a, STATIC, SINGULAR, INT32, left_angle, 18) \ -X(a, STATIC, SINGULAR, INT32, right_angle, 19) \ -X(a, STATIC, SINGULAR, INT32, robot_start, 20) \ -X(a, STATIC, SINGULAR, INT32, robot_set_speed, 21) \ -X(a, STATIC, SINGULAR, INT32, auto_mode_status, 22) \ -X(a, STATIC, SINGULAR, INT32, reason_of_robot_error, 23) \ -X(a, STATIC, SINGULAR, INT32, wire_or_wireless, 24) +X(a, STATIC, SINGULAR, INT32, Is_Online, 9) \ +X(a, STATIC, SINGULAR, INT32, Spara_Data_1, 10) \ +X(a, STATIC, SINGULAR, INT32, Spara_Data_2, 11) \ +X(a, STATIC, SINGULAR, INT32, Spara_Data_3, 12) \ +X(a, STATIC, SINGULAR, INT32, Weld_data, 13) \ +X(a, STATIC, SINGULAR, INT32, Weld_exist, 14) \ +X(a, STATIC, SINGULAR, INT32, Turn_difference, 15) \ +X(a, STATIC, SINGULAR, INT32, Present_press, 16) \ +X(a, STATIC, SINGULAR, INT32, left_angle, 17) \ +X(a, STATIC, SINGULAR, INT32, right_angle, 18) \ +X(a, STATIC, SINGULAR, INT32, robot_start, 19) \ +X(a, STATIC, SINGULAR, INT32, robot_set_speed, 20) \ +X(a, STATIC, SINGULAR, INT32, auto_mode_status, 21) \ +X(a, STATIC, SINGULAR, INT32, reason_of_robot_error, 22) \ +X(a, STATIC, SINGULAR, INT32, wire_or_wireless, 23) \ +X(a, STATIC, SINGULAR, INT32, Robot_version, 24) #define IV_struct_define_CALLBACK NULL #define IV_struct_define_DEFAULT NULL diff --git a/Core/BASE/Protobuf/Proto/bsp_Error.proto b/Core/BASE/Protobuf/Proto/bsp_Error.proto index cc87e5f..41116ba 100644 --- a/Core/BASE/Protobuf/Proto/bsp_Error.proto +++ b/Core/BASE/Protobuf/Proto/bsp_Error.proto @@ -20,13 +20,12 @@ enum ComError //枚举消息类型 Error Bit Define ZQ_CAN_ID1_LeftMotor =4; ZQ_CAN_ID2_RightMotor =5; - ZQ_CAN_ID3_SwingMotor =6; - Force_Sensor =7; + Force_Sensor =6; - Ultrasonic_Sensor =8; - Android_485 =9; //UWB_20_Error=10; - Strain_Gauge =10; - Ground_Management =11; + Ultrasonic_Sensor =7; + Android_485 =8; //UWB_20_Error=10; + Strain_Gauge =9; + Ground_Management =10; } //protoc --nanopb_out=. *.proto diff --git a/Core/BASE/Protobuf/Proto/bsp_GV.proto b/Core/BASE/Protobuf/Proto/bsp_GV.proto index 0a3079b..e321e42 100644 --- a/Core/BASE/Protobuf/Proto/bsp_GV.proto +++ b/Core/BASE/Protobuf/Proto/bsp_GV.proto @@ -48,6 +48,7 @@ message GV_struct_define int32 client_close=33; int32 robot_real_speed=34; int32 wire_status=35; + float robot_back_speed=36; //边打边退模式下机器人的后退速度 }; diff --git a/Core/BASE/Protobuf/Proto/bsp_IV.proto b/Core/BASE/Protobuf/Proto/bsp_IV.proto index 91d6aed..2bbd4d3 100644 --- a/Core/BASE/Protobuf/Proto/bsp_IV.proto +++ b/Core/BASE/Protobuf/Proto/bsp_IV.proto @@ -47,56 +47,55 @@ message IV_struct_define // 作用：上报右轮驱动电机的故障状态，用于电机故障排查 int32 Right_Motor_Err = 8; - // 摆臂电机报警状态 - // 取值规则：0 = 正常（无报警）；1 = 报警（过流/过载/堵转等） - // 作用：上报抛丸/喷砂摆臂电机的故障状态，适配摆臂速度≥25°/S的作业要求 - int32 Swing_Motor_Err = 9; + // 机器人在线状态 // 取值规则：0 = 离线（通讯中断）；1 = 在线（通讯正常） // 作用：APP/上位机判断机器人通讯状态，离线时触发声光报警 - int32 Is_Online = 10; + int32 Is_Online = 9; // 备用数据1 // 用途：预留扩展字段，可用于后续新增传感器/状态采集（如吸附压力、吸尘装置状态等） - int32 Spara_Data_1 = 11; + int32 Spara_Data_1 = 10; // 备用数据2 // 用途：预留扩展字段，可用于后续新增传感器/状态采集（如作业面温度等） - int32 Spara_Data_2 = 12; + int32 Spara_Data_2 = 11; // 备用数据3 // 用途：预留扩展字段，可用于后续新增传感器/状态采集（如急停按钮状态、电池电量等） - int32 Spara_Data_3 = 13; + int32 Spara_Data_3 = 12; //焊缝数据 - int32 Weld_data = 14; + int32 Weld_data = 13; //焊缝是否存在 - int32 Weld_exist = 15; + int32 Weld_exist = 14; //回转中心偏距 - int32 Turn_difference = 16; + int32 Turn_difference = 15; //当前压力 - int32 Present_press = 17; + int32 Present_press = 16; - int32 left_angle = 18; + int32 left_angle = 17; - int32 right_angle = 19; + int32 right_angle = 18; //检测机器人是否到达设定的启动时间 - int32 robot_start = 20; + int32 robot_start = 19; //机器人设定的速度（非实际速度） - int32 robot_set_speed=21; + int32 robot_set_speed=20; //记录是否正在进行自动行进 - int32 auto_mode_status=22; + int32 auto_mode_status=21; //确诊机器人出错的原因 - int32 reason_of_robot_error=23; + int32 reason_of_robot_error=22; //获取有线连接还是无线连接 - int32 wire_or_wireless=24; + int32 wire_or_wireless=23; + + int32 Robot_version=24; }; \ No newline at end of file diff --git a/Core/BASE/Src/BSP/B03_Para_01_100.c b/Core/BASE/Src/BSP/B03_Para_01_100.c index f6cae63..4b50708 100644 --- a/Core/BASE/Src/BSP/B03_Para_01_100.c +++ b/Core/BASE/Src/BSP/B03_Para_01_100.c @@ -29,6 +29,11 @@ B03_Para_t g_B03_param_table[100] = { .angle_offset=0.0f, .operating_times=0.0f, }, + // 第5台 B03-12 + { + .angle_offset=0.0f, + .operating_times=0.0f, + }, // 剩下97台... }; diff --git a/Core/BASE/Src/BSP/bsp_client_setting.c b/Core/BASE/Src/BSP/bsp_client_setting.c index ee776c1..03cac2a 100644 --- a/Core/BASE/Src/BSP/bsp_client_setting.c +++ b/Core/BASE/Src/BSP/bsp_client_setting.c @@ -87,6 +87,8 @@ void decode_received_data_from_client(uint8_t *buffer, uint16_t length) pb_decode(&i_pv_stream, PV_struct_define_fields, &decoded_PV_variable); HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_Serial",1); HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "Android_485", 1); + DevMon_Feed(DEV_Serial); + DevMon_Feed(DEV_YOUXIAN); //pb_decode(&i_pv_stream, PV_struct_define_fields, &decoded_PV); GV.PV = decoded_PV_variable; diff --git a/Core/BASE/Src/BSP/bsp_devic_moniter.c b/Core/BASE/Src/BSP/bsp_devic_moniter.c index 0eba249..f1f07e1 100644 --- a/Core/BASE/Src/BSP/bsp_devic_moniter.c +++ b/Core/BASE/Src/BSP/bsp_devic_moniter.c @@ -53,10 +53,18 @@ void DevMon_Init() } - //DevMon_Register(DEV_GYRO, 1000, 1000); - //DevMon_Register(DEV_SBUS, 1000, 3000); + + DevMon_Register(DEV_SBUS, 1000, 5000); + DevMon_Register(DEV_Serial, 1000, 5000); + DevMon_Register(DEV_GYRO, 1000, 5000); DevMon_Register(DEV_LEFT_MOTOR, 1000, 30000); - //DevMon_Register(DEV_RIGHT_MOTOR, 200, 4000); + DevMon_Register(DEV_RIGHT_MOTOR, 1000, 30000); + DevMon_Register(DEV_SWING_MOTOR, 1000, 30000); + //DevMon_Register(DEV_SENSOR, 1000, 5000); + //DevMon_Register(DEV_ULTRA, 1000, 5000); + DevMon_Register(DEV_YOUXIAN, 1000, 5000); + DevMon_Register(DEV_TUIGAN, 1000, 5000); + DevMon_Register(DEV_DROUND, 1000, 5000); GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, DevMon_Task); } diff --git a/Core/BASE/Src/MSP/msp_485_android.c b/Core/BASE/Src/MSP/msp_485_android.c index e9f4b86..14eae15 100644 --- a/Core/BASE/Src/MSP/msp_485_android.c +++ b/Core/BASE/Src/MSP/msp_485_android.c @@ -99,6 +99,9 @@ void decode_android_Sbus(uint8_t *buffer, uint16_t length) } HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "Android_485", 1); HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "mk32_sbus", 1); + DevMon_Feed(DEV_YOUXIAN); + DevMon_Feed(DEV_Serial); + received_android_counter++; diff --git a/Core/BASE/Src/MSP/msp_MK32_1.c b/Core/BASE/Src/MSP/msp_MK32_1.c index b52f6ed..5df73c6 100644 --- a/Core/BASE/Src/MSP/msp_MK32_1.c +++ b/Core/BASE/Src/MSP/msp_MK32_1.c @@ -60,6 +60,7 @@ void decode_MK32Data(uint8_t *buffer, uint16_t length) HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "mk32_sbus", 1); + DevMon_Feed(DEV_SBUS); diff --git a/Core/BASE/Src/MSP/msp_TL720D.c b/Core/BASE/Src/MSP/msp_TL720D.c index d8a19ac..756d8f4 100644 --- a/Core/BASE/Src/MSP/msp_TL720D.c +++ b/Core/BASE/Src/MSP/msp_TL720D.c @@ -42,6 +42,20 @@ void decode_TL720D(uint8_t *buffer, uint16_t length) if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00 && buffer[3] == 0x84) { + + + uint8_t check_sum = 0; + // 遍历除SOF(0号字节)和校验和(31号字节)之外的所有待校验字节 + for(uint8_t i = 1; i < 31; i++) + { + check_sum += buffer[i]; + } + // 校验和不匹配则直接丢弃该帧，避免错误数据被解析 + if(check_sum != buffer[31]) + { + return; + } + //SP_MSP_RF_TL720D_Parameters_In. SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll = getDeci(&buffer[4]); SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll-CV.angle_offset; @@ -58,6 +72,7 @@ void decode_TL720D(uint8_t *buffer, uint16_t length) *RobotAngle=SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll; //Is_TL720_Updating_Flag=true; HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"TL720D",1); + DevMon_Feed(DEV_GYRO); } else { //log_error("TL720D decoding failed"); diff --git a/Core/BASE/Src/MSP/msp_WH_LTE_7S0.c b/Core/BASE/Src/MSP/msp_WH_LTE_7S0.c index 1a69011..c7b3d0b 100644 --- a/Core/BASE/Src/MSP/msp_WH_LTE_7S0.c +++ b/Core/BASE/Src/MSP/msp_WH_LTE_7S0.c @@ -46,7 +46,7 @@ void WH_LTE_7S0_intialize(struct UARTHandler *Handler) void Send_WH_LTE_7S0_Data(uint8_t *data, int length) { -// char datass[256]; +// char datass[100]; // memcpy(datass, data, length); wh_LTE_7S0_Handler->UART_Decode = decode_received_data_from_computer; memcpy(wh_LTE_7S0_Handler->Tx_Buf, data, length); @@ -278,12 +278,12 @@ void LTE7S0_Init_HuaWei_MQTT(void) Send_WH_LTE_7S0_Data(cmd10, sizeof(cmd10)-1); HAL_Delay(300); - // 6. ClientId 直接复制你给出的值 + // 6. ClientId 直接复制 uint8_t cmd11[] = "AT+MQTTCID=6a447a97cbb0cf6bb96ac1a7_BHBF_ROBOT_01_0_0_2026071701\r\n"; Send_WH_LTE_7S0_Data(cmd11, sizeof(cmd11)-1); HAL_Delay(100); - // 7. Username 复制你给出的值 + // 7. Username 直接复制 uint8_t cmd12[] = "AT+MQTTUSER=6a447a97cbb0cf6bb96ac1a7_BHBF_ROBOT_01\r\n"; Send_WH_LTE_7S0_Data(cmd12, sizeof(cmd12)-1); HAL_Delay(100); @@ -372,52 +372,52 @@ void LTE7S0_Init_HuaWei_MQTT2(void) Send_WH_LTE_7S0_Data(cmd15, sizeof(cmd15)-1); HAL_Delay(100); - // 11. 开启MQTT纯透传模式 + // 11. 关闭模块心跳 uint8_t cmd16[] = "AT+HEARTEN=OFF\r\n"; Send_WH_LTE_7S0_Data(cmd16, sizeof(cmd16)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 12. 关闭遗嘱消息 uint8_t cmd17[] = "AT+MQTTWILL=0\r\n"; Send_WH_LTE_7S0_Data(cmd17, sizeof(cmd17)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 13. 向固定的话题传消息 uint8_t cmd18[] = "AT+MQTTPUBTP=4,1,$oc/devices/6a447a97cbb0cf6bb96ac1a7_BHBF_ROBOT_01/sys/messages/up,0,1\r\n"; Send_WH_LTE_7S0_Data(cmd18, sizeof(cmd18)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 14. 关闭GNSS功能 uint8_t cmd19[] = "AT+GNSSFUNEN=0\r\n"; Send_WH_LTE_7S0_Data(cmd19, sizeof(cmd19)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 15. 关闭SSL/TLS加密 uint8_t cmd20[] = "AT+SSLEN=OFF\r\n"; Send_WH_LTE_7S0_Data(cmd20, sizeof(cmd20)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 16. 设置波特率 uint8_t cmd21[] = "AT+UART=115200,8,1,NONE,0\r\n"; Send_WH_LTE_7S0_Data(cmd21, sizeof(cmd21)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 + // 17. 两条消息之间最少50ms uint8_t cmd22[] = "AT+UARTFT=50\r\n"; Send_WH_LTE_7S0_Data(cmd22, sizeof(cmd22)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 AT+MQTTPAYLOAD="" + // 18. 每条消息最多1024个字节 uint8_t cmd23[] = "AT+UARTFL=1024\r\n"; Send_WH_LTE_7S0_Data(cmd23, sizeof(cmd23)-1); HAL_Delay(300); - // 11. 开启MQTT纯透传模式 AT+MQTTPAYLOAD="" + // 19. 开启MQTT纯透传模式 AT+MQTTPAYLOAD="" uint8_t cmd24[] = "AT+MQTTPAYLOAD=""\r\n"; Send_WH_LTE_7S0_Data(cmd24, sizeof(cmd24)-1); HAL_Delay(300); - // 13. 保存全部配置到模组Flash + // 20. 保存全部配置到模组Flash uint8_t cmd25[] = "AT+S\r\n"; Send_WH_LTE_7S0_Data(cmd25, sizeof(cmd25)-1); HAL_Delay(300); @@ -465,9 +465,10 @@ int time_cut=0; char jsonBuf[256]; int send_times=0; uint8_t *send_data = NULL; -char online[8] = {0}; +char online[16] = {0}; void Upload_Data_To_HuaWeiCloud(void) { + //static char online[16]; // 1. 定义online字符串缓冲区，存储"断开"/"已连接" if(IV.Is_Online == 0) { @@ -487,12 +488,8 @@ void Upload_Data_To_HuaWeiCloud(void) GV.TL720DParameters.RF_Angle_Roll, GV.Left_Compensation, GV.Right_Compensation, - GV.Now_press / 10 + GV.Now_press/10 ); -// snprintf(jsonBuf,256, -// "{\"workStatus\":\"%s\",\"MoveSpeed\":1,\"SetSpeed\":0.1,\"Angle\":90,\"LeftCompensation\":1.1,\"RightCompensation\":-2.1,\"Press\":100}", -// online -// ); jsonBuf[255] = '\0'; // 强制末尾结束符 // 3. 发送指针直接强转，无需额外data数组 @@ -504,7 +501,7 @@ void Upload_Data_To_HuaWeiCloud(void) { time_cut = 0; // strlen获取真实报文长度，-1去掉结束符\0 - //Send_WH_LTE_7S0_Data(send_data, strlen(jsonBuf)); + //Send_WH_LTE_7S0_Data(send_data, strlen(jsonBuf)-1); uint16_t send_len = strlen(jsonBuf); if(send_data != NULL && send_len > 0) { @@ -512,7 +509,6 @@ void Upload_Data_To_HuaWeiCloud(void) send_times++; } } - //HAL_Delay(1000); } diff --git a/Core/BASE/Src/MSP/msp_ground_management.c b/Core/BASE/Src/MSP/msp_ground_management.c index 5b7d0fa..a6b0583 100644 --- a/Core/BASE/Src/MSP/msp_ground_management.c +++ b/Core/BASE/Src/MSP/msp_ground_management.c @@ -61,25 +61,37 @@ void ground_management_inquiry() &ground_management_handler->TxCount, ground_management_slave_id, 0, 8, dataToSend); - ground_management_handler->AddSendList(ground_management_handler, - ground_management_handler->Tx_Buf, - ground_management_handler->TxCount, OneLineWaitTime, NULL); + if(ground_management_handler->AddSendList != 0x0) + { + ground_management_handler->AddSendList(ground_management_handler, + ground_management_handler->Tx_Buf, + ground_management_handler->TxCount, OneLineWaitTime, NULL); + } + /***********寄存器8写德玛克电机速度*****************************/ MB_WriteHoldingReg(&ground_management_handler->Tx_Buf, &ground_management_handler->TxCount, ground_management_slave_id, 8, GV.GroundManagementValue.DMK_Speed); - ground_management_handler->AddSendList(ground_management_handler, - ground_management_handler->Tx_Buf, - ground_management_handler->TxCount, OneLineWaitTime, NULL); + if(ground_management_handler->AddSendList != 0x0) + { + ground_management_handler->AddSendList(ground_management_handler, + ground_management_handler->Tx_Buf, + ground_management_handler->TxCount, OneLineWaitTime, NULL); + } + /***********寄存器9写德玛克电机状态*****************************/ MB_WriteHoldingReg(&ground_management_handler->Tx_Buf, &ground_management_handler->TxCount, ground_management_slave_id, 9, GV.GroundManagementValue.DMK_WorkState); - ground_management_handler->AddSendList(ground_management_handler, - ground_management_handler->Tx_Buf, - ground_management_handler->TxCount, OneLineWaitTime, NULL); + if(ground_management_handler->AddSendList != 0x0) + { + ground_management_handler->AddSendList(ground_management_handler, + ground_management_handler->Tx_Buf, + ground_management_handler->TxCount, OneLineWaitTime, NULL); + } + /********************************************************************/ if (ground_management_value->Save_To_Flash == 1) @@ -96,9 +108,13 @@ void ground_management_inquiry() MB_WriteHoldingReg(&ground_management_handler->Tx_Buf, &ground_management_handler->TxCount, ground_management_slave_id, 10, 55); - ground_management_handler->AddSendList(ground_management_handler, - ground_management_handler->Tx_Buf, - ground_management_handler->TxCount, OneLineWaitTime, NULL); + if(ground_management_handler->AddSendList != 0x0) + { + ground_management_handler->AddSendList(ground_management_handler, + ground_management_handler->Tx_Buf, + ground_management_handler->TxCount, OneLineWaitTime, NULL); + } + /*****************************************************/ ground_management_value->Save_To_Flash=0; } @@ -106,8 +122,12 @@ void ground_management_inquiry() MB_ReadHoldingReg(&ground_management_handler->Tx_Buf, &ground_management_handler->TxCount, ground_management_slave_id, 0, g_m_read_count); - ground_management_handler->AddSendList(ground_management_handler, ground_management_handler->Tx_Buf, - ground_management_handler->TxCount, OneLineWaitTime, decode_ground_management); + if(ground_management_handler->AddSendList != 0x0) + { + ground_management_handler->AddSendList(ground_management_handler, ground_management_handler->Tx_Buf, + ground_management_handler->TxCount, OneLineWaitTime, decode_ground_management); + } + } void decode_ground_management(uint8_t *buffer, uint16_t length) @@ -125,6 +145,7 @@ void decode_ground_management(uint8_t *buffer, uint16_t length) HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ground_management", 1); + DevMon_Feed(DEV_DROUND); // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); } else diff --git a/Core/BASE/Src/MSP/msp_strain_gauge_new.c b/Core/BASE/Src/MSP/msp_strain_gauge_new.c index 3dbf12d..54171c4 100644 --- a/Core/BASE/Src/MSP/msp_strain_gauge_new.c +++ b/Core/BASE/Src/MSP/msp_strain_gauge_new.c @@ -126,6 +126,7 @@ void decode_strain_gauge_01(uint8_t *buffer, uint16_t length) strainGaugeValue->Pressure = (int16_t)decoded_strain_gauge_holdingReg_value[1]; HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "strain_gauge", 1); + DevMon_Feed(DEV_TUIGAN); // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); } else @@ -146,6 +147,7 @@ void decode_strain_gauge_56(uint8_t *buffer, uint16_t length) memcpy(&strainGaugeValue->RawPressure,&decoded_strain_gauge_holdingReg_value[5],4); HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "strain_gauge", 1); + DevMon_Feed(DEV_TUIGAN); // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); } else @@ -167,6 +169,7 @@ void decode_strain_gauge_09(uint8_t *buffer, uint16_t length) strainGaugeValue->Save = decoded_strain_gauge_holdingReg_value[9]; HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "strain_gauge", 1); + DevMon_Feed(DEV_TUIGAN); // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); } else diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c index 0f67b72..86ca995 100644 --- a/Core/FSM/Src/Handset_Status_Setting.c +++ b/Core/FSM/Src/Handset_Status_Setting.c @@ -65,6 +65,7 @@ static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt); /*=========================== 全局变量 ===========================*/ extern float left_compare_value; extern float right_compare_value; + // 模式-事件处理函数映射表 static const ModeEventHandler modeEventHandlers[MODE_COUNT] = { [Halt_Mode] = GetHaltModeEvents, @@ -390,6 +391,7 @@ void PV_control(void) GV.Robot_backMode = GV.PV.Robot_backMode; GV.Robot_Swing_Speed = GV.PV.Robot_Swing_Speed; GV.robot_back_distance = ((float)GV.PV.Robot_Back_Distance)/10; + GV.robot_back_speed = (((float)GV.PV.Robot_Back_Speed)/100)*6/100; //android发过来是mm/s，现在换算成了程序中统一的m/min if(GV.Robot_Move_Speed<=1) { GV.Robot_Move_Speed=1; @@ -414,7 +416,12 @@ void IV_control(void) GV.symmetricalOrNot = GV.PV.Robot_symmetricalOrNot; GV.Robot_Swing_Speed = GV.PV.Robot_Swing_Speed; GV.Robot_Move_Speed = ((P_MK32->CH11_RD1+1000)*14)/2000; - IV.SystemError = GV.SystemErrorData.Com_Error_Code; + if(IV.robot_start==2) //等到机器人成功启动了再上报错误 + { + IV.SystemError = GV.SystemErrorData.Com_Error_Code; + } + IV.Left_Motor_Err = TT_Motor[1]->TT_Motor_Fault; + IV.Right_Motor_Err = TT_Motor[2]->TT_Motor_Fault; IV.Robot_Gyro = GV.TL720DParameters.RF_Angle_Roll; IV.Is_Online = GV.P_MK32.IsOnline; IV.Robot_Move_Deri_Speed = GV.robot_real_speed; @@ -428,7 +435,7 @@ void IV_control(void) IV.right_angle = right_compare_value; IV.robot_set_speed =((float)(P_MK32->CH11_RD1+1000)*14)/2000; IV.wire_or_wireless = GV.wire_status; - + IV.Robot_version = robot_version; //机器人运行速度不允许为0，最小为1 if(IV.robot_set_speed<=1) @@ -436,6 +443,11 @@ void IV_control(void) IV.robot_set_speed=1; } + if(GV.Robot_Move_Speed<=1) + { + GV.Robot_Move_Speed=1; + } + //摆臂速度不允许为1，若设置了1，则强制置为2 if(GV.Robot_Swing_Speed==1) { diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index 0bef420..880ade6 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -95,7 +95,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = { [INPUT_ROCKER_BACKWARD] = manual_backward_group, [INPUT_ROCKER_TURN_LEFT] = manual_left_group, [INPUT_ROCKER_TURN_RIGHT] = manual_right_group, - [INPUT_KEY_AUTO_WORK_UP] = weld_auto_group, + [INPUT_KEY_AUTO_WORK_UP] = manual_auto_group,//weld_auto_group, [EMERGENCE_STOP] = Emergency_Stop_group, // 暂时用于焊缝跟踪 }, @@ -160,7 +160,7 @@ static void manual_forward_group(void) { Manually_Forward(); PaintGun_Contronl(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); } @@ -168,7 +168,7 @@ static void manual_backward_group(void) { Manually_Backward(); PaintGun_Contronl(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); } @@ -176,7 +176,7 @@ static void manual_left_group(void) { Turn_Left(); PaintGun_Contronl(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); } @@ -184,7 +184,7 @@ static void manual_right_group(void) { Turn_Right(); PaintGun_Contronl(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); } @@ -193,7 +193,7 @@ static void horizontal_forward_group(void) GV.Robot_Desired_Speed=GV.Robot_Move_Speed; horizontal_forward(); PaintGun_Contronl_Press(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); /* 若需喷枪控制，可在此添加 */ } @@ -202,7 +202,7 @@ static void horizontal_backward_group(void) GV.Robot_Desired_Speed=-GV.Robot_Move_Speed; horizontal_forward(); PaintGun_Contronl_Press(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); /* 若需喷枪控制，可在此添加 */ } @@ -241,7 +241,7 @@ static void vertical_forward_group(void) GV.Robot_Desired_Speed=GV.Robot_Move_Speed; vertical_forward(); PaintGun_Contronl_Press(); - Robot_Swing_Operation_Function(); + //(); } static void vertical_backward_group(void) @@ -249,12 +249,13 @@ static void vertical_backward_group(void) GV.Robot_Desired_Speed=-GV.Robot_Move_Speed; vertical_forward(); PaintGun_Contronl_Press(); - Robot_Swing_Operation_Function(); + //Robot_Swing_Operation_Function(); } static void vertical_auto_group(void) { // horizontal_work(); + Move_Vertical_Auto_Sub_Func(); PaintGun_Contronl_Press(); /* 若需喷枪控制，可在此添加 */ @@ -279,7 +280,8 @@ void Fsm_Init(void) // current_motor_power_state.p_state = &motor_power_off_state; - GV.GroundManagementValue.MaualControlPower=0; + GV.GroundManagementValue.MaualControlPower=0; //最一开始先交由地面端自主判断断电时机 + //GV.GroundManagementValue.MaualPowerState = 1; GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch); } @@ -297,7 +299,6 @@ void GF_Dispatch(void) if(Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == CONNECTED &&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == CONNECTED) &&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == CONNECTED) - &&(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == CONNECTED) &&Is_All_Button_Reset==1) { start_flag=1; @@ -355,12 +356,11 @@ void GF_Dispatch(void) if(robot_start_flag==0) //保证上电之后摆臂和推杆就能动,不加这句的话，就要先动其它摇杆，摆臂和推杆才能动 { - Robot_Swing_Operation_Function(); - get_swing_mode(); + //Robot_Swing_Operation_Function(); + //get_swing_mode(); PaintGun_Contronl(); } - flag_reset(); // 更新调试变量 g_debug_prev_mode = prev_mode; @@ -423,8 +423,7 @@ int AbnormalDetect(void) } /* 电机失联 */ else if(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == DISCONNECTED - || Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == DISCONNECTED - || Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == DISCONNECTED) + || Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == DISCONNECTED) { is_error = 8; } @@ -451,6 +450,7 @@ int AbnormalDetect(void) { if (cnt > 1000) { + // 触发软急停 GV.GroundManagementValue.MaualControlPower = 1; GV.GroundManagementValue.MaualPowerState = 0; @@ -485,7 +485,7 @@ int AbnormalDetect(void) // 读取指定中断的抢占/子优先级，存入全局变量 void Read_IRQ_Priority(IRQn_Type IRQn, uint32_t *preempt, uint32_t *sub) { - // 调用你官方的4参数HAL函数 + // 调用官方的4参数HAL函数 HAL_NVIC_GetPriority(IRQn, NVIC_Priority_Group, preempt, sub); } diff --git a/Core/FSM/Src/motor.c b/Core/FSM/Src/motor.c index 5134ea2..d0590d8 100644 --- a/Core/FSM/Src/motor.c +++ b/Core/FSM/Src/motor.c @@ -61,7 +61,7 @@ void Motor_Controller_intialize_CAN2(FDCANHandler *Handler) //初始化 Roughening_Motor_Controller_CAN2 = Handler; Roughening_Motor_Controller_CAN2->CAN_Decode = Roughening_MotorDecodeCAN2; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); + //HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID3_SwingMotor", 1, ComError_ZQ_CAN_ID3_SwingMotor); Roughening_DispacherController_CAN2 = Handler->dispacherController; Roughening_DispacherController_CAN2->DispacherCallTime = 2; Roughening_DispacherController_CAN2->Add_Dispatcher_List(Roughening_DispacherController_CAN2, MotorCommandsLoop_2_Position); @@ -229,6 +229,7 @@ void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) case 2: { HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1); + DevMon_Feed(DEV_RIGHT_MOTOR); TT_Analytic_Fun(2, buffer); } break; @@ -264,6 +265,7 @@ void Roughening_MotorDecodeCAN2(uint32_t canID, uint8_t *buffer, uint32_t length case 3: { HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1); + DevMon_Feed(DEV_SWING_MOTOR); TT_Analytic_Fun(3, buffer); } break; diff --git a/Core/FSM/Src/paint_gun_action.c b/Core/FSM/Src/paint_gun_action.c index e4f715c..bb070ac 100644 --- a/Core/FSM/Src/paint_gun_action.c +++ b/Core/FSM/Src/paint_gun_action.c @@ -130,6 +130,7 @@ int autoing_flag=0; int auto_count=0; int up_down=1; int auto_time=0; +//推杆自动运动函数（测试用） void tuigan_auto() { if(autoing_flag==0) //筛选时间 diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index e8971cd..6ccd4ff 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -402,7 +402,7 @@ void Robot_Stop(void) GV.auto_working=0; get_swing_mode(); //仅在机器人停的时候允许更新摆臂模式 - Robot_Swing_Operation_Function(); //机器人停的时候允许摆臂 + //Robot_Swing_Operation_Function(); //机器人停的时候允许摆臂 GV.Left_Speed_M_min = 0; GV.Right_Speed_M_min = 0; GV.turn_center_difference=0; @@ -721,24 +721,24 @@ static void handleLaneChangeContinuousRetreat(void) *-----------------------------------------------------------------*/ void Fight_Countinus_Function_Manual() { - swing_work(); + //swing_work(); //手动自动作业过程中允许改变机器人姿态 if(P_MK32->CH3_LY_H<-500) { - GV.Robot_Desired_Speed=-(float)GV.PV.Robot_Back_Speed/10; + GV.Robot_Desired_Speed=-GV.robot_back_speed; GV.Left_Speed_M_min = GV.Robot_Desired_Speed; GV.Right_Speed_M_min = GV.Robot_Desired_Speed; } else if(P_MK32->CH3_LY_H>500) { - GV.Robot_Desired_Speed=-(float)GV.PV.Robot_Back_Speed/10; + GV.Robot_Desired_Speed=-GV.robot_back_speed; GV.Left_Speed_M_min = -GV.Robot_Desired_Speed; GV.Right_Speed_M_min = -GV.Robot_Desired_Speed; } else { - GV.Robot_Desired_Speed=-(float)GV.PV.Robot_Back_Speed/10; + GV.Robot_Desired_Speed=-GV.robot_back_speed; GV.Left_Speed_M_min = GV.Robot_Desired_Speed; GV.Right_Speed_M_min = -GV.Robot_Desired_Speed; } @@ -747,21 +747,21 @@ void Fight_Countinus_Function_Manual() void Fight_Countinus_Function_Horizontal() { Update_Angle_compensation_hor(); - swing_work(); + //swing_work(); Move_Horizontal_Vertical_Task_Backwards_Do_Backward(); } void Fight_Countinus_Function_Vertical() { Update_Angle_compensation_ver(); - swing_work(); + //swing_work(); Move_Horizontal_Vertical_Task_Backwards_Do_Backward(); } void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void) { - GV.Robot_Desired_Speed=-(float)GV.PV.Robot_Back_Speed/10; + GV.Robot_Desired_Speed=-GV.robot_back_speed; auto_drive_pid_horizontal(); } @@ -769,7 +769,7 @@ void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void) void Fight_Countinus_Function_Weld() { updata_swing_angle(); - swing_work(); + //swing_work(); auto_drive_pid_weld(); } @@ -836,7 +836,7 @@ void Auto_Forward_Function_Vertical_group(void) *-----------------------------------------------------------------*/ void Fight_Alternately_Function_Manual(void) { - swing_work(); + //swing_work(); if (alternately_flag >= 0 && alternately_flag < STATE_COUNT) { if (alternately_work_manual[alternately_flag] != NULL) { @@ -853,7 +853,7 @@ void Fight_Alternately_Function_Manual(void) void Fight_Alternately_Function_Horizontal(void) { - swing_work(); + //swing_work(); if (alternately_flag >= 0 && alternately_flag < STATE_COUNT) { if (alternately_work_horizontal[alternately_flag] != NULL) { @@ -870,7 +870,7 @@ void Fight_Alternately_Function_Horizontal(void) void Fight_Alternately_Function_Vertical(void) { - swing_work(); + //swing_work(); if (alternately_flag >= 0 && alternately_flag < STATE_COUNT) { if (alternately_work_vertical[alternately_flag] != NULL) { @@ -1060,7 +1060,7 @@ static void auto_drive_pid_weld(void) avg=0; //这台机器人没有焊缝跟踪，暂时设成0用于测试模拟焊缝跟踪 float speeds[2]; - GV.Robot_Desired_Speed=-(float)GV.PV.Robot_Back_Speed/10; + GV.Robot_Desired_Speed=-GV.robot_back_speed; TwoWheel_AngleControl_Weld(avg, 0, GV.Robot_Desired_Speed,0.5, speeds); diff --git a/Core/FSM/Src/swing_action.c b/Core/FSM/Src/swing_action.c index 3d84102..344e489 100644 --- a/Core/FSM/Src/swing_action.c +++ b/Core/FSM/Src/swing_action.c @@ -115,7 +115,7 @@ void Move_Swing_Right_Func_Do_imm(void) } int32_t Position_angle; - +//摆臂电机停止函数，延时几度再停是为了避免电机频繁反转导致死机 void Move_Swing_Halt_Func_Do(void) { GV.SwingMotor.Position_immediately1_Lag2=1; diff --git a/Core/Src/main.c b/Core/Src/main.c index 48fff3f..66a34ab 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -30,6 +30,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ + #include "BHBF_ROBOT.h" #include "bsp_FDCAN.h" @@ -49,7 +50,7 @@ void Debug_Periph_NoFreeze_H7(void); /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ - +int robot_version=112; //此版本应用于焊接机器人，是由1.11版本的摆臂机器人改过来的 #define RS485_1_WaitTime 6 #define RS485_2_WaitTime 6 @@ -185,6 +186,8 @@ int main(void) //HAL_Delay(3000); //GF_BSP_GPIO_ToggleIO(Wind_IO_CTL); + + while (1) { HAL_Delay(1); @@ -389,7 +392,7 @@ void GF_Robot_Init() can2_sendListPeriod, can2_DispacherPeriod); Motor_Controller_intialize(&FD_CAN_1_Handler); - Motor_Controller_intialize_CAN2(&FD_CAN_2_Handler); + //Motor_Controller_intialize_CAN2(&FD_CAN_2_Handler); //该机器人没有摆臂电机 // LS_Motor_Controller_intialize(&FD_CAN_1_Handler); @@ -406,16 +409,15 @@ void GF_Robot_Init() // // 4. �??出配置模式，进入正常透传收发 // LTE_StepOutOfConfigMode(); - // 1. �??测模组串口�?�信 + // 1. 检测模组串口通信 //LTE_CheckModuleAlive_HuaWei(); // 2. 进入AT配置模式 //LTE_StepIntoConfigMode_HuaWei(); // 3. 下发华为云全套MQTT参数 //LTE7S0_Init_HuaWei_MQTT(); - // 4. �??出配置，模组自动发起MQTT连接云端 - LTE_StepOutOfConfigMode_HuaWei(); - // 5.连接到服务器 - //LTE_Connect_HuaWei(); + // 4. 退出配置，模组自动发起MQTT连接云端 + //LTE_StepOutOfConfigMode_HuaWei(); + } diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c index 81344ad..7ab9f72 100644 --- a/Core/Src/stm32h7xx_it.c +++ b/Core/Src/stm32h7xx_it.c @@ -96,45 +96,45 @@ void NMI_Handler(void) // // 1. 读取内核故障状�?�寄存器 // uint32_t icsr = SCB->ICSR; // 中断控制状�?�寄存器 // uint32_t cfsr = SCB->CFSR; // 配置故障状�?�寄存器 -// uint32_t hfsr = SCB->HFSR; // ?? fault 状�?�寄存器 +// uint32_t hfsr = SCB->HFSR; // �?? fault 状�?�寄存器 // uint32_t shcsr = SCB->SHCSR; // 系统 handler 控制状�?�寄存器 // -// // 2. 判断 NMI 触发?? -// //if(SCB->ICSR & (1U << 31)) // ??31 = NMI PENDING +// // 2. 判断 NMI 触发�?? +// //if(SCB->ICSR & (1U << 31)) // �??31 = NMI PENDING // //{ // // -------------------------- -// // 情况1：时钟故?? CSS 触发 +// // 情况1：时钟故�?? CSS 触发 // // -------------------------- -// if(RCC->CIFR & (1U << 8)) // RCC_CIR 寄存?? CSSF ?? = 时钟故障 +// if(RCC->CIFR & (1U << 8)) // RCC_CIR 寄存�?? CSSF �?? = 时钟故障 // { -// // 原因：外部晶?? HSE 失效 / 没起?? +// // 原因：外部晶�?? HSE 失效 / 没起�?? // NMI_Trigger_Source=1; // __NOP(); // } // // // -------------------------- -// // 情况2：外?? NMI 引脚触发 +// // 情况2：外�?? NMI 引脚触发 // // -------------------------- // else if( (RCC->CIFR & (1U << 8)) == 0 ) // { -// // 原因：外?? NMI 引脚电平触发 +// // 原因：外�?? NMI 引脚电平触发 // NMI_Trigger_Source=2; // __NOP(); // } // // // -------------------------- -// // 情况3：内??/软件触发 +// // 情况3：内�??/软件触发 // // -------------------------- // else // { -// // 内核错误、非法地??、�?�线错误?? +// // 内核错误、非法地�??、�?�线错误�?? // NMI_Trigger_Source=3; // __NOP(); // } // //} - // 死循环方便调?? + // 死循环方便调�?? while(1); /* USER CODE END NonMaskableInt_IRQn 1 */ @@ -144,12 +144,10 @@ void NMI_Handler(void) * @brief This function handles Hard fault interrupt. */ void HardFault_Handler(void) -{ + { /* USER CODE BEGIN HardFault_IRQn 0 */ - - /* USER CODE END HardFault_IRQn 0 */ while (1) {