From d8559546396d3aebfac7b3435ce9e2a0bd69fab4 Mon Sep 17 00:00:00 2001 From: "LAPTOPNUM\\lapto" <1141383048@qq.com> Date: Mon, 1 Jun 2026 17:04:39 +0800 Subject: [PATCH] first commit --- .project | 2 +- .settings/language.settings.xml | 4 +- Core/BASE/Protobuf/PSource/bsp_GV.pb.h | 2 +- .../Protobuf/PSource/bsp_strain_gauge.pb.h | 17 +- .../Protobuf/Proto/bsp_strain_gauge.proto | 3 + Core/BASE/Src/BSP/bsp_MB_host.c | 4 +- Core/BASE/Src/MSP/msp_ground_management.c | 5 +- Core/BASE/Src/MSP/msp_strain_gauge.c | 147 ++++- Core/FSM/Inc/fsm_state_control.h | 1 + Core/FSM/Inc/robot_move_actions.h | 9 + Core/FSM/Src/change_line_control.c | 85 ++- Core/FSM/Src/fsm_state_control.c | 612 +++++++++++------- Core/FSM/Src/motors.c | 363 +++++++++-- Core/FSM/Src/robot_move_actions.c | 68 +- Core/Inc/main.h | 1 + Core/Inc/stm32h7xx_hal_conf.h | 2 +- Core/Inc/stm32h7xx_it.h | 1 + Core/Src/dma.c | 14 +- Core/Src/fdcan.c | 4 +- Core/Src/i2c.c | 4 +- Core/Src/main.c | 8 +- Core/Src/stm32h7xx_it.c | 14 + Core/Src/tim.c | 2 +- Core/Src/usart.c | 16 +- GP_Floor_Roughening Debug.launch | 80 +++ ...BB_WiredRPM.ioc => GP_Floor_Roughening.ioc | 49 +- LWIP/Target/ethernetif.c | 6 +- Roughening_UDPV3_0BB_WiredRPM Debug.launch | 80 +++ readme.txt | 3 + 29 files changed, 1192 insertions(+), 414 deletions(-) create mode 100644 GP_Floor_Roughening Debug.launch rename Roughening_UDPV2_0BB_WiredRPM.ioc => GP_Floor_Roughening.ioc (95%) create mode 100644 Roughening_UDPV3_0BB_WiredRPM Debug.launch diff --git a/.project b/.project index aa4a67c..a9337a8 100644 --- a/.project +++ b/.project @@ -1,6 +1,6 @@ - Roughening_UDPV2_0BB_WiredRPM + GP_Floor_Roughening diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 1823b1f..de6bd90 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index a5ba5e6..1ffbbd3 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -111,7 +111,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 1506 +#define GV_struct_define_size 1539 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h b/Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h index d1839a4..e9ca6a6 100644 --- a/Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h @@ -17,6 +17,9 @@ typedef struct _Strain_Gauge_Struct { int32_t HX711_K; /* 寄存器2 HX711_K */ int32_t HX711_D; /* 寄存器3 HX711_D */ int32_t Save; /* 寄存器9： 设置为55时，把当前寄存器输入存入flash，写入成功自动变成1 */ + int32_t RawPressure; /* 寄存器1： 输出的传感器原始数据 */ + int32_t Read_K; + int32_t Read_D; } Strain_Gauge_Struct; @@ -25,8 +28,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define Strain_Gauge_Struct_init_default {0, 0, 0, 0, 0} -#define Strain_Gauge_Struct_init_zero {0, 0, 0, 0, 0} +#define Strain_Gauge_Struct_init_default {0, 0, 0, 0, 0, 0, 0, 0} +#define Strain_Gauge_Struct_init_zero {0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define Strain_Gauge_Struct_MotorControl_tag 1 @@ -34,6 +37,9 @@ extern "C" { #define Strain_Gauge_Struct_HX711_K_tag 3 #define Strain_Gauge_Struct_HX711_D_tag 4 #define Strain_Gauge_Struct_Save_tag 5 +#define Strain_Gauge_Struct_RawPressure_tag 6 +#define Strain_Gauge_Struct_Read_K_tag 7 +#define Strain_Gauge_Struct_Read_D_tag 8 /* Struct field encoding specification for nanopb */ #define Strain_Gauge_Struct_FIELDLIST(X, a) \ @@ -41,7 +47,10 @@ X(a, STATIC, SINGULAR, INT32, MotorControl, 1) \ X(a, STATIC, SINGULAR, INT32, Pressure, 2) \ X(a, STATIC, SINGULAR, INT32, HX711_K, 3) \ X(a, STATIC, SINGULAR, INT32, HX711_D, 4) \ -X(a, STATIC, SINGULAR, INT32, Save, 5) +X(a, STATIC, SINGULAR, INT32, Save, 5) \ +X(a, STATIC, SINGULAR, INT32, RawPressure, 6) \ +X(a, STATIC, SINGULAR, INT32, Read_K, 7) \ +X(a, STATIC, SINGULAR, INT32, Read_D, 8) #define Strain_Gauge_Struct_CALLBACK NULL #define Strain_Gauge_Struct_DEFAULT NULL @@ -52,7 +61,7 @@ extern const pb_msgdesc_t Strain_Gauge_Struct_msg; /* Maximum encoded size of messages (where known) */ #define BSP_STRAIN_GAUGE_PB_H_MAX_SIZE Strain_Gauge_Struct_size -#define Strain_Gauge_Struct_size 55 +#define Strain_Gauge_Struct_size 88 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto b/Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto index 8ca943b..3c39c3a 100644 --- a/Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto +++ b/Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto @@ -7,6 +7,9 @@ message Strain_Gauge_Struct int32 HX711_K=3; //寄存器2 HX711_K int32 HX711_D=4; //寄存器3 HX711_D int32 Save=5;//寄存器9： 设置为55时，把当前寄存器输入存入flash，写入成功自动变成1 + int32 RawPressure=6;// 寄存器1： 输出的传感器原始数据 + int32 Read_K=7; + int32 Read_D=8; }; diff --git a/Core/BASE/Src/BSP/bsp_MB_host.c b/Core/BASE/Src/BSP/bsp_MB_host.c index af33fc9..37a05da 100644 --- a/Core/BASE/Src/BSP/bsp_MB_host.c +++ b/Core/BASE/Src/BSP/bsp_MB_host.c @@ -325,12 +325,14 @@ void MB_WriteNumHoldingReg(uint8_t *Tx_Buf, uint8_t *TxCount_t, uint8_t _addr, * 返 回 值: 1 表示解析正确，0 表示解析错误 CRC验证错误 * 说 明: */ + uint8_t MB_Decode_HoldingRegs(uint8_t buffer[], uint16_t length,uint16_t Read_Reg_Num,uint16_t* Decoded_Reg_Value) { /* CRC 校验 */ uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]); /* CRC 校验正确 */ - if (crc_check == MB_CRC16(buffer, length - 2)) + + if (crc_check == MB_CRC16(buffer, length - 2) && (buffer[1]&0xf0)!=0x80) { int i = 0; for (i = 0; i < Read_Reg_Num; i++) diff --git a/Core/BASE/Src/MSP/msp_ground_management.c b/Core/BASE/Src/MSP/msp_ground_management.c index b6f1b84..c59879c 100644 --- a/Core/BASE/Src/MSP/msp_ground_management.c +++ b/Core/BASE/Src/MSP/msp_ground_management.c @@ -46,13 +46,16 @@ void ground_management_inquiry() dataToSend[1] = SWAP_ENDIAN_16((uint16_t ) ground_management_value->K2); dataToSend[2] = SWAP_ENDIAN_16((uint16_t ) ground_management_value->K3); dataToSend[3] = SWAP_ENDIAN_16((uint16_t ) ground_management_value->K4); + +// ground_management_value->K5_Default = 0; // Default置0才能给输出 dataToSend[4] = SWAP_ENDIAN_16((uint16_t ) ground_management_value->K5_Default); + dataToSend[5] = SWAP_ENDIAN_16( (uint16_t ) ground_management_value->MaualControlPower); dataToSend[6] = SWAP_ENDIAN_16( (uint16_t ) ground_management_value->MaualPowerState); - ground_management_value->Time_Out_Period=400; + ground_management_value->Time_Out_Period=1000; dataToSend[7] = SWAP_ENDIAN_16( (uint16_t ) ground_management_value->Time_Out_Period); diff --git a/Core/BASE/Src/MSP/msp_strain_gauge.c b/Core/BASE/Src/MSP/msp_strain_gauge.c index 1af20c3..d7acc40 100644 --- a/Core/BASE/Src/MSP/msp_strain_gauge.c +++ b/Core/BASE/Src/MSP/msp_strain_gauge.c @@ -11,10 +11,15 @@ Strain_Gauge_Struct *strainGaugeValue; uint8_t strain_gauge_slave_id = 0x32; //默认为1 struct UARTHandler *strain_gauge_handler; +uint16_t to_send_bytes[50]; + DispacherController *strain_gauge_dispacherController; void strain_gauge_loop(); void decode_strain_gauge_01(uint8_t *buffer, uint16_t length); void decode_strain_gauge_09(uint8_t *buffer, uint16_t length); +void decode_strain_gauge_56(uint8_t *buffer, uint16_t length); +void decode_strain_gauge_2(uint8_t *buffer, uint16_t length); +void decode_strain_gauge_34(uint8_t *buffer, uint16_t length); void strain_gauge_intialize(struct UARTHandler *Handler) { //strain_gauge_slave_id=slave_id; @@ -24,7 +29,7 @@ void strain_gauge_intialize(struct UARTHandler *Handler) strain_gauge_dispacherController->Dispacher_Enable = 1; //不周期性发送 strain_gauge_dispacherController->Add_Dispatcher_List( - strain_gauge_dispacherController, strain_gauge_loop);//Dispatcher_List_Add_t bsp com helper.c + strain_gauge_dispacherController, strain_gauge_loop); //Dispatcher_List_Add_t bsp com helper.c HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "strain_gauge", 0, ComError_Strain_Gauge); @@ -46,48 +51,78 @@ void strain_gauge_loop() MB_ReadHoldingReg(&strain_gauge_handler->Tx_Buf, &strain_gauge_handler->TxCount, strain_gauge_slave_id, 1, 1); //03 command ; read 3 registers 从1 开始 读取1个 strain_gauge_handler->AddSendList(strain_gauge_handler, - strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, OneLineWaitTime, - decode_strain_gauge_01); + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, decode_strain_gauge_01); //推杆控制 MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf, &strain_gauge_handler->TxCount, strain_gauge_slave_id, 0, - strainGaugeValue->MotorControl);//strainGaugeValue->MotorControl 电机控制，=0 停止，=1 前进，=2 后退 + strainGaugeValue->MotorControl); //strainGaugeValue->MotorControl 电机控制，=0 停止，=1 前进，=2 后退 strain_gauge_handler->AddSendList(strain_gauge_handler, - strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, OneLineWaitTime, + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, NULL); /*写寄存器2 3 KD**/ - if(SAVE_Register23==1) + if (SAVE_Register23 == 1) { /*写寄存器2 K**/ MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf, - &strain_gauge_handler->TxCount, strain_gauge_slave_id, 2, - strainGaugeValue->HX711_K); + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 2, + (uint16_t) strainGaugeValue->HX711_K); strain_gauge_handler->AddSendList(strain_gauge_handler, - strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, OneLineWaitTime, - NULL); - /*写寄存器3 D**/ - MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf, - &strain_gauge_handler->TxCount, strain_gauge_slave_id, 3, - strainGaugeValue->HX711_D); + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, + NULL); + /*写寄存器3 and 4 D**/ + memcpy(&to_send_bytes[3],&strainGaugeValue->HX711_D,4); + + to_send_bytes[3]=SWAP_ENDIAN_16(to_send_bytes[3]); + to_send_bytes[4]=SWAP_ENDIAN_16(to_send_bytes[4]); + MB_WriteNumHoldingReg(&strain_gauge_handler->Tx_Buf, + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 3, 2, + &to_send_bytes[3]); + + strain_gauge_handler->AddSendList(strain_gauge_handler, + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, + NULL); +// read 5 -6 holidng registers origin press + MB_ReadHoldingReg(&strain_gauge_handler->Tx_Buf, + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 5, 2); //03 command ; read 2 registers 从1 开始 读取1个 strain_gauge_handler->AddSendList(strain_gauge_handler, - strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, OneLineWaitTime, - NULL); - SAVE_Register23=0; + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, decode_strain_gauge_56); + SAVE_Register23 = 0; } /*写寄存器9 设置为55保存KD**/ - if(SAVE_Register9==1) + if (SAVE_Register9 == 1) { - + strainGaugeValue->Save=55; MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf, - &strain_gauge_handler->TxCount, strain_gauge_slave_id, 9, - strainGaugeValue->Save); + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 9, + strainGaugeValue->Save); strain_gauge_handler->AddSendList(strain_gauge_handler, - strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, OneLineWaitTime, - NULL); - SAVE_Register9=0; + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, + NULL); + SAVE_Register9 = 0; + } + if(1 == Read_KD) + { + /*read K 2 D 3-4*/ + MB_ReadHoldingReg(&strain_gauge_handler->Tx_Buf, + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 2, 1); //03 command ; read 2 registers 从1 开始 读取1个 + strain_gauge_handler->AddSendList(strain_gauge_handler, + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, decode_strain_gauge_2); + MB_ReadHoldingReg(&strain_gauge_handler->Tx_Buf, + &strain_gauge_handler->TxCount, strain_gauge_slave_id, 3, 2); //03 command ; read 2 registers 从1 开始 读取1个 + strain_gauge_handler->AddSendList(strain_gauge_handler, + strain_gauge_handler->Tx_Buf, strain_gauge_handler->TxCount, + OneLineWaitTime, decode_strain_gauge_34); + Read_KD = 0 ; } } @@ -103,7 +138,7 @@ void decode_strain_gauge_01(uint8_t *buffer, uint16_t length) &decoded_strain_gauge_holdingReg_value[1]); if (decoded_result == 1) { - strainGaugeValue->Pressure = decoded_strain_gauge_holdingReg_value[1]; + strainGaugeValue->Pressure = (int16_t)decoded_strain_gauge_holdingReg_value[1]; HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "strain_gauge", 1); // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); @@ -113,13 +148,73 @@ void decode_strain_gauge_01(uint8_t *buffer, uint16_t length) LOGFF(DL_ERROR, "strain_gauge_decoding failed"); } } +/*origin value*/ +void decode_strain_gauge_56(uint8_t *buffer, uint16_t length) +{ + int decoded_result = MB_Decode_HoldingRegs(buffer, length, 2, + &decoded_strain_gauge_holdingReg_value[5]); + if (decoded_result == 1) + { + +// decoded_strain_gauge_holdingReg_value[5]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[5]); +// decoded_strain_gauge_holdingReg_value[6]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[6]); + memcpy(&strainGaugeValue->RawPressure,&decoded_strain_gauge_holdingReg_value[5],4); + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, + "strain_gauge", 1); + // LOG("Battery_sensor succeeded and the force is %d", *CMCU06_ForceValue); + } + else + { + LOGFF(DL_ERROR, "strain_gauge_decoding failed"); + } +} +/*k value*/ +void decode_strain_gauge_2(uint8_t *buffer, uint16_t length) +{ + int decoded_result = MB_Decode_HoldingRegs(buffer, length, 1, + &decoded_strain_gauge_holdingReg_value[2]); + if (decoded_result == 1) + { + +// decoded_strain_gauge_holdingReg_value[5]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[5]); +// decoded_strain_gauge_holdingReg_value[6]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[6]); + memcpy(&strainGaugeValue->Read_K,&decoded_strain_gauge_holdingReg_value[2],2); + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, + "strain_gauge", 1); + } + else + { + LOGFF(DL_ERROR, "strain_gauge_decoding failed"); + } +} +/*D value*/ +void decode_strain_gauge_34(uint8_t *buffer, uint16_t length) +{ + int decoded_result = MB_Decode_HoldingRegs(buffer, length, 2, + &decoded_strain_gauge_holdingReg_value[3]); + if (decoded_result == 1) + { + +// decoded_strain_gauge_holdingReg_value[5]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[5]); +// decoded_strain_gauge_holdingReg_value[6]=SWAP_ENDIAN_16(decoded_strain_gauge_holdingReg_value[6]); + memcpy(&strainGaugeValue->Read_D,&decoded_strain_gauge_holdingReg_value[3],4); + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, + "strain_gauge", 1); + + } + else + { + LOGFF(DL_ERROR, "strain_gauge_decoding failed"); + } +} + //读取 09的寄存器 void decode_strain_gauge_09(uint8_t *buffer, uint16_t length) { // uint8_t data1[100]; // memcpy(data1, buffer, length); - int decoded_result = MB_Decode_HoldingRegs(buffer, length, 2, + int decoded_result = MB_Decode_HoldingRegs(buffer, length, 1, &decoded_strain_gauge_holdingReg_value[9]); if (decoded_result == 1) { diff --git a/Core/FSM/Inc/fsm_state_control.h b/Core/FSM/Inc/fsm_state_control.h index ef68688..abae38b 100644 --- a/Core/FSM/Inc/fsm_state_control.h +++ b/Core/FSM/Inc/fsm_state_control.h @@ -16,6 +16,7 @@ extern void RougheningControl(); extern int Auto_TiltControl(); extern void IV_control(); extern void MoveControl(); +extern void Pressure_Safety_Monitor(void); extern double GetVehicleSpeed(double speed_selection); extern void joysticker_manual_control(); diff --git a/Core/FSM/Inc/robot_move_actions.h b/Core/FSM/Inc/robot_move_actions.h index 8bcea4e..a02a80a 100644 --- a/Core/FSM/Inc/robot_move_actions.h +++ b/Core/FSM/Inc/robot_move_actions.h @@ -38,6 +38,11 @@ extern void Move_Head_To_Down_Do(transition_t *p_this); extern void Move_Head_To_Left_Do(transition_t *p_this); extern void Move_Head_To_Right_Do(transition_t *p_this); +extern void Move_Head_To_UP_Add_Adjust_Do(transition_t *p_this); +extern void Move_Head_To_Left_Add_Adjust_Do(transition_t *p_this); +extern void Move_Head_To_Right_Add_Adjust_Do(transition_t *p_this); + + extern void HALT_State_Enter(transition_t *p_this); extern void HALT_State_Exit(transition_t *p_this); @@ -76,4 +81,8 @@ extern transition_state_t robot_move_head_to_up_enum_state; extern transition_state_t robot_move_head_to_right_enum_state; extern transition_state_t robot_move_head_to_down_enum_state; +extern transition_state_t robot_move_head_to_up_add_adjust_enum_state; +extern transition_state_t robot_move_head_to_left_add_adjust_enum_state; +extern transition_state_t robot_move_head_to_right_add_adjust_enum_state; + #endif /* FSM_INC_ROBOT_MOVE_ACTIONS_H_ */ diff --git a/Core/FSM/Src/change_line_control.c b/Core/FSM/Src/change_line_control.c index 00e9f5c..c41a6e3 100644 --- a/Core/FSM/Src/change_line_control.c +++ b/Core/FSM/Src/change_line_control.c @@ -74,6 +74,16 @@ int LaneChangeControl_Rough() if (P_MK32->CH4_SA == -1000) /*竖直上or水平换道*/ { + // 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权 + if (GV.PV.RunMode == Move_Manual) + { + return 0; + } +// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/ +// { +// return 0; +// } + if (GV.PV.RunMode == Move_Horizontal_Move)/*水平换道*/ { if (HorizontalLaneChangeState != Lane_Change_Start) @@ -95,11 +105,13 @@ int LaneChangeControl_Rough() return 1; } /*************/ - if (GV.PV.RunMode != Move_Vertical_Move_To_Left - && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/ - { - return 1; - } + + + +// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/ +// { +// return 1; +// } if (VerticalLaneChangeState != Lane_Change_Start) { @@ -123,6 +135,17 @@ int LaneChangeControl_Rough() if (P_MK32->CH4_SA == 1000) //竖直下换道 { + + // 防止无模式下，SB按下后，之后按下SA，SA抢占SB控制权 + if (GV.PV.RunMode == Move_Manual) + { + return 0; + } +// if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right)/*竖直换道*/ +// { +// return 0; +// } + if (GV.PV.RunMode != Move_Vertical_Move_To_Left && GV.PV.RunMode != Move_Vertical_Move_To_Right) { @@ -200,16 +223,19 @@ void Horizontal_Lane_Change_Turn_To_Right_Control() if (CompareTimer(LaneChangeWaittime, &timer_handler_1)) { CurrentHorizontal_ChangeState = HorizontalChange_TurnToRight; - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_right_enum_state); /*转到朝上*/ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_right_enum_state); /*转到朝上*/ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/ + } break; } case HorizontalChange_TurnToRight: { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_right_enum_state); /*转到朝右*/ - if (abs(GV.Robot_Angle - CV.RobotRightAngleValue) +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_right_enum_state); /*转到朝右*/ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/ + if (abs(GV.Robot_Angle - (CV.RobotRightAngleValue + GV.PV.Vertical_Calibration)) <= CV.Allowable_Error_For_Angle_Tracking) { CurrentHorizontal_ChangeState = HorizontalChange_End; @@ -268,16 +294,21 @@ void Horizontal_Lane_Change_Turn_To_Left_Control() if (CompareTimer(LaneChangeWaittime, &timer_handler_1)) //计时结束 { CurrentHorizontal_ChangeState = HorizontalChange_TurnToLeft; - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_left_enum_state); /*转到朝左*/ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_left_enum_state); /*转到朝左*/ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/ + + } break; } case HorizontalChange_TurnToLeft: { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_left_enum_state); /*转到朝左*/ - if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue) +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_left_enum_state); /*转到朝左*/ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/ + + if (abs(GV.Robot_Angle - (CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration)) <= CV.Allowable_Error_For_Angle_Tracking) { CurrentHorizontal_ChangeState = HorizontalChange_End; @@ -350,8 +381,11 @@ void Vertical_Lane_Change_From_Left_To_Right_UP_Control() if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ + + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */ + } else { @@ -422,8 +456,9 @@ void Vertical_Lane_Change_From_Left_To_Right_Down_Control() if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内 { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */ } else { @@ -493,8 +528,10 @@ void Vertical_Lane_Change_From_Right_To_Left_UP_Control() if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */ + } else { @@ -565,8 +602,10 @@ void Vertical_Lane_Change_From_Right_To_Left_Down_Control() if (abs(GV.Robot_Angle - (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)) >= CV.Allowable_Error_For_Angle_Tracking) //误差在1度内 { - fsm_state_set(¤t_robot_move_state, - &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ +// fsm_state_set(¤t_robot_move_state, +// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */ + fsm_state_set(¤t_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */ + } else { diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index 1dc6698..29eb104 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -36,7 +36,8 @@ int LaneChangeControl(); void DHRougheningControl(); //拉毛前端控制 void Mannual_TiltControl(); -void Mannual_TiltControl1(); +//void Mannual_TiltControl1(); +void Mannual_TiltControl2(); int Auto_TiltControl(); @@ -44,6 +45,7 @@ int Auto_TiltControl(); void IV_control(); void MoveControl(); +void Pressure_Safety_Monitor(void); int LaneChangeControl(); @@ -92,6 +94,7 @@ int RegionAuto_MoveTask_Test_Result = 0; void Fsm_Init() { + IV_control(); //机器人运动状态初始化 fsm_state_init(¤t_robot_move_state, &robot_halt_state); @@ -102,8 +105,13 @@ void Fsm_Init() //机器人电机供电状态初始化 fsm_state_init(¤t_motor_power_state, &motor_power_off_state); + + GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch); IV.RobotRestart = 1; //机器人上电初始化，需要通知机器人 + + + } void GF_Dispatch() { @@ -120,7 +128,11 @@ void GF_Dispatch() { return; } - /***上电检测通过 电机上电**/ + + /* 移动到Fsm_Init()里，实现软急停断48V后，只能重新上电恢复。 + * 但移上去出现有点问题：，电机、推杆首次无法正常启动，需排查初次上电完整启动流程后才能考虑插入 + * */ +// /***上电检测通过 电机上电**/ fsm_state_set(¤t_motor_power_state, &motor_power_on_state); /* 上电关 此时设为开 */ /*按下遥控 app无法选择*/ @@ -205,41 +217,104 @@ void GF_Dispatch() + + + + + + + + + + + + + + // ///* -// * MoveControl版本2：配置了两种模式：到达压力预警之前和之后 -// * 之前——所有功能都能正常运行； -// * 之后——仅保留推杆手动操作，其余全部关停，且手动操作仅支持推杆抬升 +// * 三级压力报警版本V2.0 +// * // * */ -//// 全局定义 +// //volatile uint8_t g_IsPressureLocked = 0; // +//// 压力保护参数配置（可动态调试修改） +//volatile int g_Pressure_Lock_Threshold = 5000; // 锁定阈值：触发报警锁定的压力值 +//volatile int g_Pressure_Warn_Threshold; // 预警阈值：限制下压动作的压力值 +//volatile int g_Pressure_Unlock_Threshold; // 解锁阈值：解除锁定的压力值（滞回区间） +// //void MoveControl() //{ -// // 1. 故障与压力检测 -// if (MotorErrorDetect() == 1 || IV.Press >= 500) -// { -// fsm_state_set(¤t_robot_move_state, &robot_halt_state); -// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// // ============================================================ +// // 【配置参数】 +// // ============================================================ +// const int COUNT_LIMIT = 250; // 0.5s 时间滤波 +// +// g_Pressure_Warn_Threshold = g_Pressure_Lock_Threshold - 20; +// g_Pressure_Unlock_Threshold = g_Pressure_Lock_Threshold - 50; +// +// +// static int pressure_over_count = 0; +// bool needs_restricted_mode = false; // 标记是否需要受限模式 +// +// // ============================================================ +// // 1. 压力检测与锁定逻辑 +// // ============================================================ // -// if (IV.Press >= 500) { -// SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert); -// g_IsPressureLocked = 1; // 进入过载锁定状态 +// // 使用全局变量：g_Pressure_Lock_Threshold +// if (IV.Press >= g_Pressure_Lock_Threshold) +// { +// pressure_over_count++; +// if (pressure_over_count >= COUNT_LIMIT) +// { +// g_IsPressureLocked = 1; +// if (pressure_over_count > COUNT_LIMIT + 50) pressure_over_count = COUNT_LIMIT + 50; +// } +// } +// else +// { +// if (g_IsPressureLocked == 1) +// { +// // 使用全局变量：g_Pressure_Unlock_Threshold +// if (IV.Press < g_Pressure_Unlock_Threshold) +// { +// g_IsPressureLocked = 0; +// pressure_over_count = 0; +// } +// else +// { +// // 保持在锁定状态，压力在 解锁值~锁定值 之间 +// pressure_over_count = 0; +// } // } -// if (MotorErrorDetect() == 1) { -// g_IsPressureLocked = 1; +// else +// { +// pressure_over_count = 0; // } // } +// +// // 电机错误（原注释代码保持不变） +// // if (MotorErrorDetect() == 1) +// // { +// // fsm_state_set(¤t_robot_move_state, &robot_halt_state); +// // fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// // g_IsPressureLocked = 1; +// // } +// +// // 报警位处理 +// if (g_IsPressureLocked == 1) +// { +// SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert); +// fsm_state_set(¤t_robot_move_state, &robot_halt_state); +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +// } // else // { // SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert); -// // 增加迟滞，防止在500附近频繁切换，比如低于480才解除 -// if (g_IsPressureLocked == 1 && IV.Press < 480) { -// g_IsPressureLocked = 0; // 解除锁定，恢复正常 -// } // } // -// // ... (补偿控制和速度计算代码保持不变) ... +// // 补偿与速度计算（保持不变） // Lcompensation_control(); // Rcompensation_control(); // speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; @@ -247,94 +322,207 @@ void GF_Dispatch() // IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); // // // ============================================================ -// // 根据锁定状态选择摇杆控制函数 +// // 2. 决定模式并强制中断旧状态 // // ============================================================ -// if (g_IsPressureLocked == 1) +// +// // 判断是否需要受限模式 (锁定中 OR 压力过高预警) +// // 使用全局变量：g_Pressure_Warn_Threshold +// if (g_IsPressureLocked == 1 || IV.Press >= g_Pressure_Warn_Threshold) // { -// // 压力过载状态：调用受限的摇杆控制（只能抬，不能压） -// Mannual_TiltControl1(); +// needs_restricted_mode = true; +// +// // 核心动作：在切换函数前，强制将推杆停止 +// fsm_state_set(¤t_tilt_state, &tilt_halt_state); // } // else // { -// // 正常工作状态：调用正常的摇杆控制 -// Mannual_TiltControl(); +// needs_restricted_mode = false; // } +// // ============================================================ +// // 3. 执行对应的摇杆控制函数 +// // ============================================================ +// if (needs_restricted_mode) +// { +// // 此时 current_tilt_state 已经是 tilt_halt_state 了 +// // Mannual_TiltControl1 将在这个“停止”的基础上运行 +// // 逻辑是：检测到下压角度 -> 忽略/保持停止；检测到上抬角度 -> 允许上抬 +// // Mannual_TiltControl1(); // -// // ============================================================ -// // 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回 -// // ============================================================ -// if (g_IsPressureLocked == 1) -// { -// // 再次确保状态机处于停止态（双重保险） -// fsm_state_set(¤t_robot_move_state, &robot_halt_state); -// fsm_state_set(¤t_roughening_state, &roughening_halt_state); -// -// // 直接退出，不执行后面的自动巡航、换道、手动行走等代码 -// return; -// } +// // 更改为压力自动恢复正常值:回归到屏幕设定的压力值内 +// Mannual_TiltControl2(); +// } +// else +// { +// // 正常模式，无干预 +// Mannual_TiltControl(); +// } // -// // --- 以下代码只有在未锁定 (g_IsPressureLocked == 0) 时才会执行 --- +// // ============================================================ +// // 4. 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回。暂时关闭这个选项，因为有自适应压力调节，所以不需要锁定机器人运动 +// // ============================================================ +//// if (g_IsPressureLocked == 1) +//// { +// // 暂时关闭这个选项，因为有自适应压力调节，所以不需要锁定机器人运动 +//// fsm_state_set(¤t_robot_move_state, &robot_halt_state); +//// fsm_state_set(¤t_roughening_state, &roughening_halt_state); +//// return; +//// } // -// DHRougheningControl(); /* 拉毛盘控制 */ +// // ============================================================ +// // 5. 正常运动逻辑 +// // ============================================================ +// DHRougheningControl(); // -// if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) -// { -// Rough_RegionAutoMoveControl(); -// return; -// } +// if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) +// { +// Rough_RegionAutoMoveControl(); +// return; +// } // -// if (LaneChangeControl_Rough() != 0) -// { -// return; -// } +// if (LaneChangeControl_Rough() != 0) +// { +// return; +// } // -// if (P_MK32->CH5_SB == -1000) -// { -// robot_forwards(); -// return; -// } -// if (P_MK32->CH5_SB == 1000) -// { -// robot_backwards(); -// return; -// } +// if (P_MK32->CH5_SB == -1000) +// { +// robot_forwards(); +// return; +// } +// if (P_MK32->CH5_SB == 1000) +// { +// robot_backwards(); +// return; +// } // -// joysticker_manual_control(); // 摇杆手动控制底盘 +// joysticker_manual_control(); +// // ================================================= //} + + + /* - * MoveControl版本7：彻底解决状态机惯性问题 - * 核心逻辑： - * 1. 在调用摇杆函数 BEFORE，先根据压力状态判断是否需要“强制急停”推杆。 - * 2. 如果需要保护，先手动 fsm_state_set(..., tilt_halt_state)，切断之前的下压指令。 - * 3. 然后再调用 Mannual_TiltControl1()，此时它将在“停止”的基础上工作，只能响应“上抬”。 + * 三级压力报警与震荡检测模块 V2.2 + * 功能：适配低频传感器（10/40Hz），通过降采样实现高频控制下的震荡检测 */ -// 全局定义 +// 全局状态变量 volatile uint8_t g_IsPressureLocked = 0; -int PRESSURE_LOCK_THRESHOLD = 1500; -void MoveControl() +// 压力保护参数配置 +volatile int g_Pressure_Lock_Threshold = 5000; // 锁定阈值 +volatile int g_Pressure_Warn_Threshold = 0; // 预警阈值 +volatile int g_Pressure_Unlock_Threshold = 0; // 解锁阈值 + +// 【新增】震荡检测配置参数 +// 假设本函数每2ms调用一次： +// 设为 1 -> 每2ms检测一次 (适合高频传感器) +// 设为 5 -> 每10ms检测一次 (适合100Hz传感器) +// 设为 20 -> 每40ms检测一次 (适合25Hz传感器) +//volatile uint8_t g_Pressure_Oscillation_Step = 5; +volatile int g_Pressure_Oscillation_Step = 5; + + +volatile int OSCILLATION_THRESHOLD = 1000; // 震荡判定阈值：相邻两次采样差值超过此值视为异常 (根据实际传感器量程调整) +volatile int OSCILLATION_CONFIRM_COUNT = 5; // 震荡确认次数：连续5次(10ms)检测到剧烈跳变即触发 +volatile int pressure_over_count = 0; +volatile int last_sampled_pressure = 0; // 上一次“有效采样点”的压力 +volatile int oscillation_count = 0; // 震荡计数器 +//volatile uint8_t sample_timer = 0; // 降采样计时器 +volatile int sample_timer = 0; + +volatile int diff = 120;//测试是否真的能做出震荡判断 + + +void Pressure_Safety_Monitor(void) { // ============================================================ // 【配置参数】 // ============================================================ -// const int PRESSURE_LOCK_THRESHOLD = 1500; - const int PRESSURE_UNLOCK_THRESHOLD = 1450; - const int COUNT_LIMIT = 10; // 0.2s + const int COUNT_LIMIT = 250; // 0.5s 持续超压滤波 +// const int OSCILLATION_THRESHOLD = 1000; // 震荡判定阈值：差值绝对值 +// const int OSCILLATION_CONFIRM_COUNT = 5; // 震荡确认次数 + + // 动态计算滞回区间：即三级压力预警的剩余两个值 + g_Pressure_Warn_Threshold = g_Pressure_Lock_Threshold - 20; + g_Pressure_Unlock_Threshold = g_Pressure_Lock_Threshold - 50; + + // 静态变量 // 震荡检测专用静态变量 +// static int pressure_over_count = 0; +// static int last_sampled_pressure = 0; // 上一次“有效采样点”的压力 +// static int oscillation_count = 0; // 震荡计数器 +// static uint8_t sample_timer = 0; // 降采样计时器 + + int current_press = IV.Press; + + // ============================================================ + // 1. 降采样震荡检测逻辑 + // ============================================================ + sample_timer++; + + // 只有当计时器达到设定步长时，才进行一次“有效对比” + if (sample_timer >= g_Pressure_Oscillation_Step) + { +// int diff = current_press - last_sampled_pressure; + diff = current_press - last_sampled_pressure; + if (diff < 0) diff = -diff; // 取绝对值 + + // 判断是否剧烈跳变 + if (diff >= OSCILLATION_THRESHOLD) + { + oscillation_count++; + } + else + { + // 波动恢复正常，震荡计数清零 + oscillation_count = 0; + } - static int pressure_over_count = 0; - bool needs_restricted_mode = false; // 标记是否需要受限模式 + last_sampled_pressure = current_press; + sample_timer = 0; + } // ============================================================ - // 1. 压力检测与锁定逻辑 + // 2. 状态机逻辑 (修复解锁漏洞) // ============================================================ - if (IV.Press >= PRESSURE_LOCK_THRESHOLD) + // --- 情况 A：如果当前已经锁定 --- + if (g_IsPressureLocked == 1) + { + // 判断是否可以解锁 + // 逻辑：必须同时满足两个条件才算安全 + // 1. 压力值已经降下来 (解除超压风险) + // 2. 震荡已经消失 (解除乱跳风险) + + bool is_pressure_low = (current_press < g_Pressure_Unlock_Threshold); + bool is_oscillation_free = (oscillation_count == 0); + + if (is_pressure_low && is_oscillation_free) + { + // 只有既不乱跳，压力又低，才允许解锁 + g_IsPressureLocked = 0; + pressure_over_count = 0; + } + // 否则保持锁定状态，直接返回，不再执行下面的超压计数逻辑 + return; + } + + // --- 情况 B：当前未锁定，检查是否需要触发锁定 --- + + // 1. 优先检查震荡 (震荡优先级高于超压) + if (oscillation_count >= OSCILLATION_CONFIRM_COUNT) + { + g_IsPressureLocked = 1; + return; // 锁定后直接返回 + } + + // 2. 检查持续超压 + if (current_press >= g_Pressure_Lock_Threshold) { pressure_over_count++; if (pressure_over_count >= COUNT_LIMIT) @@ -345,34 +533,23 @@ void MoveControl() } else { - if (g_IsPressureLocked == 1) - { - if (IV.Press < PRESSURE_UNLOCK_THRESHOLD) - { - g_IsPressureLocked = 0; - pressure_over_count = 0; - } - else - { - // 保持在锁定状态，压力在 4500-5000 之间 - pressure_over_count = 0; - } - } - else - { - pressure_over_count = 0; - } + pressure_over_count = 0; } +} -// 电机错误 -// if (MotorErrorDetect() == 1) -// { -// fsm_state_set(¤t_robot_move_state, &robot_halt_state); -// fsm_state_set(¤t_roughening_state, &roughening_halt_state); -// g_IsPressureLocked = 1; -// } - // 报警位 + +// ============================================================ +// 主控制函数 +// ============================================================ +void MoveControl() +{ + // 1. 【核心隔离】调用压力监测函数 + Pressure_Safety_Monitor(); + + bool needs_restricted_mode = false; + + // 2. 压力传感器报警位处理 if (g_IsPressureLocked == 1) { SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert); @@ -384,24 +561,17 @@ void MoveControl() SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert); } + // 3. 补偿与速度计算 Lcompensation_control(); Rcompensation_control(); speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200; GV.Robot_Move_Speed = speed_M_min_toE01_M_min(GetVehicleSpeed(speed_selection)); IV.RobotMoveSpeed = GetVehicleSpeed(speed_selection); - // ============================================================ - // 2. 决定模式并强制中断旧状态 - // ============================================================ - - // 判断是否需要受限模式 (锁定中 OR 压力过高预警) - // 把阈值设在 4800，一旦超过，就算没锁定，也先限制下压，防止冲过5000 - if (g_IsPressureLocked == 1 || IV.Press >= 1480) + // 4. 决定模式 + if (g_IsPressureLocked == 1 || IV.Press >= g_Pressure_Warn_Threshold) { needs_restricted_mode = true; - - // 核心动作：在切换函数前，强制将推杆状态机复位为“停止” ：否则，即使超限，因为右摇杆一直没有松手，所以还是能keep下压 - // 这行代码会立即打断 Mannual_TiltControl 之前可能设置的 tilt_down_state fsm_state_set(¤t_tilt_state, &tilt_halt_state); } else @@ -409,35 +579,17 @@ void MoveControl() needs_restricted_mode = false; } - // ============================================================ - // 3. 执行对应的摇杆控制函数 - // ============================================================ + // 5. 执行控制 if (needs_restricted_mode) { - // 此时 current_tilt_state 已经是 tilt_halt_state 了 - // Mannual_TiltControl1 将在这个“停止”的基础上运行 - // 逻辑是：检测到下压角度 -> 忽略/保持停止；检测到上抬角度 -> 允许上抬 - Mannual_TiltControl1(); + Mannual_TiltControl2(); } else { - // 正常模式，无干预 Mannual_TiltControl(); } - // ============================================================ - // 4. 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回 - // ============================================================ - if (g_IsPressureLocked == 1) - { - fsm_state_set(¤t_robot_move_state, &robot_halt_state); - fsm_state_set(¤t_roughening_state, &roughening_halt_state); - return; - } - - // ============================================================ - // 5. 正常运动逻辑 - // ============================================================ + // 6. 运动逻辑 DHRougheningControl(); if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move) @@ -470,6 +622,8 @@ void MoveControl() + + /* @brief: 电机出现报警返回1，电机正常返回0 */ uint8_t MotorErrorDetect() { @@ -618,109 +772,114 @@ void joysticker_manual_control() - -void Mannual_TiltControl() -{ - - if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; - - if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance - && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ - { - fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ - return; - } - int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; - - if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) - { - fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ - return; - } - // 原下压逻辑：无限位 - if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/ - { - if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet) - { - fsm_state_set(¤t_tilt_state, &tilt_down_state); - return; - } - fsm_state_set(¤t_tilt_state, &tilt_halt_state); - } - - - -// // 下压+软限位。消抖计数，初始化为0 -// static int pressure_over_count = 0; // -// const int SOFT_LIMIT_PRESSURE = 2000; // 压力阈值 -// const int COUNT_LIMIT = 1; // 200ms / 2ms = 100次计数 -// if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance) +//void Mannual_TiltControl() +//{ +// +// if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; +// +// if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance +// && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ // { -// int current_pressure = IV.Press; +// fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ +// return; +// } +// int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; // -// // --- 新增软限位 (带消抖) --- -// bool is_soft_safe = (current_pressure <= SOFT_LIMIT_PRESSURE); -// if (!is_soft_safe) -// { -// pressure_over_count++; -// -// // 判断是否达到连续时间阈值 (100次 * 2ms = 200ms) -// if (pressure_over_count >= COUNT_LIMIT) -// { -// fsm_state_set(¤t_tilt_state, &tilt_halt_state); -// // 此时计数器保持在高位，直到压力降低或摇杆回中才清零 -// } -// else -// { -// // 处于消抖过程中 (例如超压了 50ms)，视为干扰，允许继续下降 -// fsm_state_set(¤t_tilt_state, &tilt_down_state); -// } -// } -// else +// if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) +// { +// fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ +// return; +// } +// // 原下压逻辑：无限位 +// if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/ +// { +// if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet) // { -// // 压力正常， 计数器清零 (打破连续性) -// pressure_over_count = 0; -// -// // 允许下降 // fsm_state_set(¤t_tilt_state, &tilt_down_state); +// return; // } -// -// return; +// fsm_state_set(¤t_tilt_state, &tilt_halt_state); // } // -// // 其他角度情况，默认停止 -// fsm_state_set(¤t_tilt_state, &tilt_halt_state); +//} + + +void Mannual_TiltControl() +{ + if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; + // 摇杆回中 + if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance + && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance) + { + fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ + return; + } + int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; + // 上升 + if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) + { + fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ + return; + } + // 下降 + if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance) + { + if (GV.PV.PressSet == 0) + { + // 情况1: 预设值为0，对应用户没有输入预期压力,故允许一直下压，直到达到极限压力值的 80% + if (GV.Strain_Gauge.Pressure <= (g_Pressure_Lock_Threshold * 0.8)) + { + fsm_state_set(¤t_tilt_state, &tilt_down_state); + } + else + { + fsm_state_set(¤t_tilt_state, &tilt_halt_state); + } + } + else + { + // 情况2: 预设值不为0，即用户有输入，能压到哪里受限于屏幕预设值 + if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet) + { + fsm_state_set(¤t_tilt_state, &tilt_down_state); + } + else + { + fsm_state_set(¤t_tilt_state, &tilt_halt_state); + } + } + } } -// 压力过载下的推杆控制：删除下压功能 -void Mannual_TiltControl1() +volatile int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值，后续改成界面上压力设定值 +// 压力过载下的推杆控制：自动模式下，一旦超出软限位值， +void Mannual_TiltControl2() { + // 基础模式检查 + if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; - if (TiltWorking_Mode != Tilt_Manual_Mode || GV.PV.RunMode == 0) return; +// const int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值 - if (abs(P_MK32->CH1_RY_V) <= CV.Joy_Sticker_Value_Allowance - && abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)/*停止*//*600*/ - { - fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ - return; - } - int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI; + // 1. 自动抬升逻辑 + // 只要压力还大于停止阈值，就一直保持抬升状态 + if (IV.Press > g_AUTO_LIFT_RELEASE_PRESSURE) + { + fsm_state_set(¤t_tilt_state, &tilt_up_state); /*强制上升*/ + return; + } - if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance) - { - fsm_state_set(¤t_tilt_state, &tilt_up_state);/*上升*/ - return; - } + // 2. 停止逻辑 + // 当压力降到停止阈值以下，停止推杆 + fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*停止推杆*/ } - int Auto_TiltControl() { if (0 == isAutoAdjustPress) @@ -729,8 +888,7 @@ int Auto_TiltControl() } TiltWorking_Mode = Tilt_Auto_Mode; /*存在一个问题，当显示大于设定值时需要上升，但是由于机械硬件无法上升，压力无法改变，会卡在推杆上升中，自动程序无法执行*/ - if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 - 50 - && GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8 + 50) + if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 ) { fsm_state_set(¤t_tilt_state, &tilt_halt_state); /*关闭推杆*/ /*正转开启拉毛盘*/ @@ -742,13 +900,13 @@ int Auto_TiltControl() isAutoAdjustPress = 0; return 0; } - else if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 + 50) - { - fsm_state_set(¤t_tilt_state, &tilt_up_state); /*上升推杆*/ - return 1; - - } - else if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8 - 50) +// else if (GV.Strain_Gauge.Pressure >= GV.PV.PressSet * 0.8 + 50) +// { +// fsm_state_set(¤t_tilt_state, &tilt_up_state); /*上升推杆*/ +// return 1; +// +// } + else if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8) { fsm_state_set(¤t_tilt_state, &tilt_down_state); /*下降推杆*/ return 1; @@ -904,6 +1062,16 @@ int AbnormalDetect() fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/ return 1; } + // 预留：初次上电时，检测压力传感器值是否在零位附近如【-50，+50】 +// if (IV.Press >= -50 && IV.Press <= 50) +// { +//// fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/ +// fsm_state_set(¤t_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/ +// return 1; +// } + + + if (P_MK32->IsOnline == 0) //等于0时 subus有数，但是遥控器关机了，或者失联 { fsm_state_set(¤t_robot_move_state, &robot_halt_state); /*停止机器人*/ diff --git a/Core/FSM/Src/motors.c b/Core/FSM/Src/motors.c index 9ca0354..47d1343 100644 --- a/Core/FSM/Src/motors.c +++ b/Core/FSM/Src/motors.c @@ -1,3 +1,137 @@ +///* +// * motors.c +// * +// * Created on: 2025年12月29日 +// * Author: xsq +// */ +// +//#include "motors.h" +//#include "msp_TTMotor_ZQ.h" +// +//char TT_Motor_Need_To_Activate = 0; +// +//FDCANHandler *Roughening_Motor_Controller; +//DispacherController *Roughening_DispacherController; +//TT_MotorParameters *TT_Motor[7]; +//#define LeftMotorID 1 +//#define RightMotorID 2 +//int32_t speed_E01m_per_min(int32_t speed_E01m_per_min); +// +//void MotorCommandsLoop(); +//void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length); +// +//void Roughening_Motor_Controller_intialize(FDCANHandler *Handler) +//{ +// //初始化 +// +// Roughening_Motor_Controller = Handler; +// Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; +// +// HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, +// "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor); +// HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, +// "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor); +// +// Roughening_DispacherController = Handler->dispacherController; +// Roughening_DispacherController->DispacherCallTime = 2; +// Roughening_DispacherController->Add_Dispatcher_List( +// Roughening_DispacherController, MotorCommandsLoop); +// +// LOGFF(DL_WARN,"TT_Motors_intialize"); +// +//} +//void MotorCommandsLoop() +//{ +// +// if (TT_Motor_Need_To_Activate == 1) +// { +// +// ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); +// ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); +// ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); +// ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); +// +// SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); +// SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); +//// SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); +//// SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); +// TT_Motor_Need_To_Activate = 2; +// } +// else if (TT_Motor_Need_To_Activate == 2) +// { +// for (int i = 1; i < 3; i++) +// { +// TT_Request_Position(i, Roughening_Motor_Controller, 6); +// TT_Request_Velocity(i, Roughening_Motor_Controller, 6); +// TT_Request_Current(i, Roughening_Motor_Controller, 6); +// TT_Request_Fault(i, Roughening_Motor_Controller, 6); +// TT_Request_Tempature(i, Roughening_Motor_Controller, 6); +// } +// TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6, +// speed_E01m_per_min(GV.LeftMotor.Target_Velcity) ); +// TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6, +// speed_E01m_per_min(-GV.RightMotor.Target_Velcity) ); +// +// } +// +//} +// +//char a[10]; +//void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) +//{ +// +// memcpy(a,buffer,8); +// switch (canID - 0x580) +// { +// +// case LeftMotorID: +// { +// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, +// "ZQ_CAN_ID2_LeftMotor", 1); +// TT_Analytic_Fun(LeftMotorID, buffer); +// } +// break; +// case RightMotorID: +// { +// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, +// "ZQ_CAN_ID3_RightMotor", 1); +// TT_Analytic_Fun(RightMotorID, buffer); +// } +// break; +// } +// +//} +///* +// * SpeedMPMin:0.1 m/min +// * return:1 pulse/s +// * */ +//int32_t speed_E01m_per_min(int32_t speed_E01m_per_min) +//{ +// return (int32_t) (speed_E01m_per_min*0.1 *10 * CV.wheel_Reduction_Ratio +// / (3.14 * CV.wheel_Diameter_m)); +//} + + + + + + + + + + + + + + + + + + +/** + * motors.c版本02：仿照摆臂的心跳模式，有问题，左电机死掉了 + * + * / /* * motors.c * @@ -13,100 +147,189 @@ char TT_Motor_Need_To_Activate = 0; FDCANHandler *Roughening_Motor_Controller; DispacherController *Roughening_DispacherController; TT_MotorParameters *TT_Motor[7]; + #define LeftMotorID 1 #define RightMotorID 2 + +// --- 定义心跳包 CAN ID (0x700 + 节点ID) --- +#define HEARTBEAT_ID_LEFT 0x701 +#define HEARTBEAT_ID_RIGHT 0x702 + int32_t speed_E01m_per_min(int32_t speed_E01m_per_min); void MotorCommandsLoop(); void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length); -void Roughening_Motor_Controller_intialize(FDCANHandler *Handler) +// --- 1. 心跳包发送函数 --- +void Send_Motor_Heartbeat(int32_t can_id, FDCANHandler *handler) +{ + handler->Tx_Buf[0] = 0x05; // 0x05 = Operational + handler->AddCANSendList(handler, can_id, 1, handler->Tx_Buf, 0, NULL); +} + +// --- 2. 配置心跳监控 (Consumer Heartbeat) --- +void Configure_Asynchronous_Mode(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) +{ + ZQ_Motor_Controller->Tx_Buf[0] = 0x23; // SDO 写入 + ZQ_Motor_Controller->Tx_Buf[1] = 0x16; // 0x1016 + ZQ_Motor_Controller->Tx_Buf[2] = 0x10; + ZQ_Motor_Controller->Tx_Buf[3] = 0x01; +// ZQ_Motor_Controller->Tx_Buf[4] = 0xe8; // 1000ms +// ZQ_Motor_Controller->Tx_Buf[5] = 0x03; + ZQ_Motor_Controller->Tx_Buf[4] = 0x64; // 100ms + ZQ_Motor_Controller->Tx_Buf[5] = 0x00; + ZQ_Motor_Controller->Tx_Buf[6] = Node_Number; // 监听 ID + ZQ_Motor_Controller->Tx_Buf[7] = 0x00; + + ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x600 + MotorID, 8, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); + +} + +// --- 3. 新增：NMT 启动函数 --- +void Enable_NMT(int32_t MotorID, FDCANHandler *ZQ_Motor_Controller, int32_t Node_Number, int32_t WaitTime) { - //初始化 + ZQ_Motor_Controller->Tx_Buf[0] = 0x01; // 0x01 = Start Node + ZQ_Motor_Controller->Tx_Buf[1] = Node_Number; + // 发送到 0x000 (NMT 广播地址) + ZQ_Motor_Controller->AddCANSendList(ZQ_Motor_Controller, 0x000, 2, ZQ_Motor_Controller->Tx_Buf, WaitTime, NULL); - Roughening_Motor_Controller = Handler; - Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, - "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor); - HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, - "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor); +} - Roughening_DispacherController = Handler->dispacherController; - Roughening_DispacherController->DispacherCallTime = 2; - Roughening_DispacherController->Add_Dispatcher_List( - Roughening_DispacherController, MotorCommandsLoop); +void Roughening_Motor_Controller_intialize(FDCANHandler *Handler) +{ + Roughening_Motor_Controller = Handler; + Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN; + + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, + "ZQ_CAN_ID2_LeftMotor", 0, ComError_ZQ_LeftMotor); + HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, + "ZQ_CAN_ID3_RightMotor", 0, ComError_ZQ_RightMotor); - LOGFF(DL_WARN,"TT_Motors_intialize"); + Roughening_DispacherController = Handler->dispacherController; + Roughening_DispacherController->DispacherCallTime = 2; + Roughening_DispacherController->Add_Dispatcher_List( + Roughening_DispacherController, MotorCommandsLoop); + LOGFF(DL_WARN,"TT_Motors_intialize"); } + void MotorCommandsLoop() { + static int heartbeat_counter = 0; + + if (TT_Motor_Need_To_Activate == 1) + { + // 1. 激活电机 (内部可能包含状态机切换) + ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); + ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); - if (TT_Motor_Need_To_Activate == 1) - { - - ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); - ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); - ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); - ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); - - SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); - SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 1000, 1000, 0); -// SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); -// SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 3500, 450, 0); - TT_Motor_Need_To_Activate = 2; - } - else if (TT_Motor_Need_To_Activate == 2) - { - for (int i = 1; i < 3; i++) - { - TT_Request_Position(i, Roughening_Motor_Controller, 6); - TT_Request_Velocity(i, Roughening_Motor_Controller, 6); - TT_Request_Current(i, Roughening_Motor_Controller, 6); - TT_Request_Fault(i, Roughening_Motor_Controller, 6); - TT_Request_Tempature(i, Roughening_Motor_Controller, 6); - } - TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6, - speed_E01m_per_min(GV.LeftMotor.Target_Velcity) ); - TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6, - speed_E01m_per_min(-GV.RightMotor.Target_Velcity) ); - - } + ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); + ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); +// ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000); +// ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000); + // 3. 【新增】发送 NMT 启动指令 + // 这一步是必须的，确保电机进入运行状态 + Enable_NMT(000, Roughening_Motor_Controller, 1, 1000); + Enable_NMT(000, Roughening_Motor_Controller, 2, 1000); + + + // 2. 配置心跳监控 + // 告诉电机：监听 ID 1 和 2，超时 1000ms + Configure_Asynchronous_Mode(LeftMotorID, Roughening_Motor_Controller, 1, 500); + Configure_Asynchronous_Mode(RightMotorID, Roughening_Motor_Controller, 2, 500); + + + // 4. 配置速度模式 + SpeedModeSetup(LeftMotorID, Roughening_Motor_Controller, 6, 500, 500, 0); + SpeedModeSetup(RightMotorID, Roughening_Motor_Controller, 6, 500, 500, 0); + + // 5. 立即发送一次心跳 + Send_Motor_Heartbeat(HEARTBEAT_ID_LEFT, Roughening_Motor_Controller); + Send_Motor_Heartbeat(HEARTBEAT_ID_RIGHT, Roughening_Motor_Controller); + + + + + + TT_Motor_Need_To_Activate = 2; + } + else if (TT_Motor_Need_To_Activate == 2) + { + // 读取状态 + for (int i = 1; i < 3; i++) + { + TT_Request_Position(i, Roughening_Motor_Controller, 6); + TT_Request_Velocity(i, Roughening_Motor_Controller, 6); + TT_Request_Current(i, Roughening_Motor_Controller, 6); + TT_Request_Fault(i, Roughening_Motor_Controller, 6); + TT_Request_Tempature(i, Roughening_Motor_Controller, 6); + } + + // 速度控制 + TT_SpeedMode_Set_TargetSpeed(LeftMotorID, Roughening_Motor_Controller, 6, + speed_E01m_per_min(GV.LeftMotor.Target_Velcity) ); + TT_SpeedMode_Set_TargetSpeed(RightMotorID, Roughening_Motor_Controller, 6, + speed_E01m_per_min(-GV.RightMotor.Target_Velcity) ); + + // 周期性心跳 + heartbeat_counter++; + if (heartbeat_counter >= 5) + { + Send_Motor_Heartbeat(HEARTBEAT_ID_LEFT, Roughening_Motor_Controller); + Send_Motor_Heartbeat(HEARTBEAT_ID_RIGHT, Roughening_Motor_Controller); + heartbeat_counter = 0; + } + } } + + + char a[10]; void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length) { - - memcpy(a,buffer,8); - switch (canID - 0x580) - { - - case LeftMotorID: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, - "ZQ_CAN_ID2_LeftMotor", 1); - TT_Analytic_Fun(LeftMotorID, buffer); - } - break; - case RightMotorID: - { - HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, - "ZQ_CAN_ID3_RightMotor", 1); - TT_Analytic_Fun(RightMotorID, buffer); - } - break; - } - + memcpy(a,buffer,8); + switch (canID - 0x580) + { + case LeftMotorID: + { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, + "ZQ_CAN_ID2_LeftMotor", 1); + TT_Analytic_Fun(LeftMotorID, buffer); + } + break; + case RightMotorID: + { + HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, + "ZQ_CAN_ID3_RightMotor", 1); + TT_Analytic_Fun(RightMotorID, buffer); + } + break; + } } -/* - * SpeedMPMin:0.1 m/min - * return:1 pulse/s - * */ + int32_t speed_E01m_per_min(int32_t speed_E01m_per_min) { - return (int32_t) (speed_E01m_per_min*0.1 *10 * CV.wheel_Reduction_Ratio - / (3.14 * CV.wheel_Diameter_m)); + return (int32_t) (speed_E01m_per_min*0.1 *10 * CV.wheel_Reduction_Ratio + / (3.14 * CV.wheel_Diameter_m)); } + + + + + + + + + + + + + + + + + + diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index a86e94c..05cb0cf 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -35,6 +35,11 @@ transition_state_t robot_move_head_to_up_enum_state={NULL,Move_Head_To_UP_Do,N transition_state_t robot_move_head_to_right_enum_state={NULL,Move_Head_To_Right_Do,NULL}; /* 移动至头朝右 */ transition_state_t robot_move_head_to_down_enum_state={NULL,Move_Head_To_Down_Do,NULL}; /* 移动至头朝下 */ +transition_state_t robot_move_head_to_up_add_adjust_enum_state={NULL,Move_Head_To_UP_Add_Adjust_Do,NULL};//移动到头朝上+竖直微调角度 +transition_state_t robot_move_head_to_left_add_adjust_enum_state={NULL,Move_Head_To_Left_Add_Adjust_Do,NULL}; /* 移动至头朝左+竖直微调角度 */ +transition_state_t robot_move_head_to_right_add_adjust_enum_state={NULL,Move_Head_To_Right_Add_Adjust_Do,NULL}; /* 移动至头朝右+竖直微调角度 */ + + /* 声明最底层函数 */ void Move_Forwards_Do(int32_t Target_Angle); @@ -47,15 +52,19 @@ char IsRobotHaltSateChangedFlag = 0; void Forwards_State_Do(transition_t *p_this) { - GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed; - GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed; + // 计算拨杆带来的额外差速修正值，范围变为 -10 到 10,%10才是真实的+—1 + int correction = ((P_MK32->CH3_LY_H + 1000) * 20) / 2000 - 10; + + GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed + correction; + GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed - correction; } void Backwards_State_Do(transition_t *p_this) { - GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed; - GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed; + int correction = ((P_MK32->CH3_LY_H + 1000) * 20) / 2000 - 10; + GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - correction; + GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + correction; } /* 向左转 * @@ -120,7 +129,10 @@ void HALT_State_Exit(transition_t *p_this) void Move_Horizontal_Task_Forwards_Right_Do(transition_t *p_this) { // Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation); - Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); //+补偿+水平微调 + // 修改项V1.0：水平移动+左右补偿+水平微调 +// Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); //+补偿+水平微调 +// 修改项V2.0：水平移动，共用竖直微调这一补偿项，即删除替换原来的水平微调。水平移动+左右补偿+竖直微调 + Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration); //+补偿+竖直微调 } void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this) @@ -133,7 +145,9 @@ void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this) void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this) { // Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation); - Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); +// Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); + // 修改项V2.0： + Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration); } /**和拉毛区分开 防止左右补偿加到喷漆上**/ @@ -147,7 +161,9 @@ void Move_Horizontal_Task_Backwards_Right_Paint_Do(transition_t *p_this) void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this) { // Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation); - Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); +// Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration); + // 修改项V2.0： + Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration); } /**和拉毛区分开 防止左右补偿加到喷漆上**/ @@ -162,7 +178,9 @@ void Move_Horizontal_Task_Forwards_Left_Paint_Do(transition_t *p_this) void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this) { // Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation); - Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); +// Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); + // 修改项V2.0： + Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration); } void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this) @@ -176,10 +194,17 @@ void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this) * */ void Move_Head_To_UP_Do(transition_t *p_this) { -// Calbrate_Robot_Positon(CV.RobotUpAngleValue); // 原始版本，转到固定的0/90度那种 + Calbrate_Robot_Positon(CV.RobotUpAngleValue); // 原始版本，转到固定的0/90度那种 +// Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)); // 加入竖直微调项的转动角度 +} + +void Move_Head_To_UP_Add_Adjust_Do(transition_t *p_this) +{ Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)); // 加入竖直微调项的转动角度 } + + /* 原地PID转向至目标角度 CV.RobotDownAngleValue * Target_Angle： 目标角度 （单位 0.01 度） * */ @@ -194,16 +219,33 @@ void Move_Head_To_Down_Do(transition_t *p_this) void Move_Head_To_Left_Do(transition_t *p_this) { Calbrate_Robot_Positon(CV.RobotLeftAngleValue); +// Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平 + +} + +void Move_Head_To_Left_Add_Adjust_Do(transition_t *p_this) +{ + Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平 + } + + /* 原地PID转向至目标角度 CV.RobotRightAngleValue * Target_Angle： 目标角度 （单位 0.01 度） * */ void Move_Head_To_Right_Do(transition_t *p_this) { Calbrate_Robot_Positon(CV.RobotRightAngleValue); +// Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平 + } +void Move_Head_To_Right_Add_Adjust_Do(transition_t *p_this) +{ + Calbrate_Robot_Positon(CV.RobotRightAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平 + +} /*******************以下纠偏底层函数**********************************/ /* @@ -293,15 +335,15 @@ void Calbrate_Robot_Positon(int Target_Angle) else if (abs(GV.Robot_Angle - Target_Angle) <= 500) //误差在正负1度内 1-5° { dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 1, 0,0.5, 10); - GV.LeftMotor.Target_Velcity = -dletAngle ; - GV.RightMotor.Target_Velcity = dletAngle; + GV.LeftMotor.Target_Velcity = -dletAngle/2 ; /* 换道回正or转向时，最后的5度内，转速降低 */ + GV.RightMotor.Target_Velcity = dletAngle/2; } else//大于2° { dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 2, 0, 0.5, 40); - GV.LeftMotor.Target_Velcity = -dletAngle ; - GV.RightMotor.Target_Velcity = dletAngle ; + GV.LeftMotor.Target_Velcity = -dletAngle/2;/* 换道回正or转向时，最后的5度内，转速降低 */ + GV.RightMotor.Target_Velcity = dletAngle/2 ; } } diff --git a/Core/Inc/main.h b/Core/Inc/main.h index d01fb4d..1dc4026 100644 --- a/Core/Inc/main.h +++ b/Core/Inc/main.h @@ -159,6 +159,7 @@ typedef enum _HardWare_Disconnected_State CONNECTED = 0, DISCONNECTED = 1, } HardWare_Disconnected_State; extern int SAVE_Register23; //设置KD +extern int Read_KD; extern int SAVE_Register9; //设置55 保存KD #define UR7 1 diff --git a/Core/Inc/stm32h7xx_hal_conf.h b/Core/Inc/stm32h7xx_hal_conf.h index 9458184..ddda1fd 100644 --- a/Core/Inc/stm32h7xx_hal_conf.h +++ b/Core/Inc/stm32h7xx_hal_conf.h @@ -165,7 +165,7 @@ * @brief This is the HAL system configuration section */ #define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY (15UL) /*!< tick interrupt priority */ +#define TICK_INT_PRIORITY (0UL) /*!< tick interrupt priority */ #define USE_RTOS 0 #define USE_SD_TRANSCEIVER 0U /*!< use uSD Transceiver */ #define USE_SPI_CRC 0U /*!< use CRC in SPI */ diff --git a/Core/Inc/stm32h7xx_it.h b/Core/Inc/stm32h7xx_it.h index a318c99..b750ce0 100644 --- a/Core/Inc/stm32h7xx_it.h +++ b/Core/Inc/stm32h7xx_it.h @@ -72,6 +72,7 @@ void TIM8_UP_TIM13_IRQHandler(void); void UART4_IRQHandler(void); void UART5_IRQHandler(void); void ETH_IRQHandler(void); +void ETH_WKUP_IRQHandler(void); void USART6_IRQHandler(void); void UART7_IRQHandler(void); void QUADSPI_IRQHandler(void); diff --git a/Core/Src/dma.c b/Core/Src/dma.c index 94e8552..48501c7 100644 --- a/Core/Src/dma.c +++ b/Core/Src/dma.c @@ -44,25 +44,25 @@ void MX_DMA_Init(void) /* DMA interrupt init */ /* DMA1_Stream0_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn); /* DMA1_Stream1_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); /* DMA1_Stream2_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); /* DMA1_Stream3_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn); /* DMA1_Stream4_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn); /* DMA1_Stream5_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn); /* DMA1_Stream6_IRQn interrupt configuration */ - HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 0, 0); + HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 3, 0); HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn); } diff --git a/Core/Src/fdcan.c b/Core/Src/fdcan.c index 5936457..6453e8d 100644 --- a/Core/Src/fdcan.c +++ b/Core/Src/fdcan.c @@ -165,7 +165,7 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* FDCAN1 interrupt Init */ - HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 0, 0); + HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 2, 0); HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); /* USER CODE BEGIN FDCAN1_MspInit 1 */ @@ -205,7 +205,7 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* FDCAN2 interrupt Init */ - HAL_NVIC_SetPriority(FDCAN2_IT0_IRQn, 0, 0); + HAL_NVIC_SetPriority(FDCAN2_IT0_IRQn, 2, 0); HAL_NVIC_EnableIRQ(FDCAN2_IT0_IRQn); /* USER CODE BEGIN FDCAN2_MspInit 1 */ diff --git a/Core/Src/i2c.c b/Core/Src/i2c.c index 01c12d2..b072e1f 100644 --- a/Core/Src/i2c.c +++ b/Core/Src/i2c.c @@ -106,9 +106,9 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) __HAL_RCC_I2C4_CLK_ENABLE(); /* I2C4 interrupt Init */ - HAL_NVIC_SetPriority(I2C4_EV_IRQn, 0, 0); + HAL_NVIC_SetPriority(I2C4_EV_IRQn, 15, 0); HAL_NVIC_EnableIRQ(I2C4_EV_IRQn); - HAL_NVIC_SetPriority(I2C4_ER_IRQn, 0, 0); + HAL_NVIC_SetPriority(I2C4_ER_IRQn, 15, 0); HAL_NVIC_EnableIRQ(I2C4_ER_IRQn); /* USER CODE BEGIN I2C4_MspInit 1 */ diff --git a/Core/Src/main.c b/Core/Src/main.c index 5c92a59..f4d62bd 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -75,6 +75,7 @@ int can2_DispacherPeriod = 10; int SAVE_To_CV = 0; int SAVE_Register9=0; //设置55 保存KD int SAVE_Register23=0; //设置KD +int Read_KD=0; /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ @@ -114,8 +115,7 @@ int main(void) /* USER CODE END 1 */ /* MPU Configuration--------------------------------------------------------*/ - -MPU_Config(); + MPU_Config(); /* Enable I-Cache---------------------------------------------------------*/ SCB_EnableICache(); @@ -182,7 +182,7 @@ MPU_Config(); if (SAVE_To_CV == 1) { SAVE_To_CV = 0; - //CV写入falsh�???????? + //CV写入falsh�????????? GF_BSP_EEPROM_Set_CV(CV); CV = GF_BSP_EEPROM_Get_CV(); //FS_SetZero(); @@ -333,7 +333,7 @@ void GF_Robot_Init() tcp_server_init(3490); dLT_Log_intialize_udp_tcp(); - TL720D_intialize(&RS_485_1_UART_Handler);//倾角�? 115200 + TL720D_intialize(&RS_485_1_UART_Handler);//倾角�?? 115200 // upper_Computer_UART_Handler_intialize(&RS_485_4_UART_Handler); android_485_intialize(&RS_485_2_UART_Handler); diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c index 2a9a1d9..ea176df 100644 --- a/Core/Src/stm32h7xx_it.c +++ b/Core/Src/stm32h7xx_it.c @@ -461,6 +461,20 @@ void ETH_IRQHandler(void) /* USER CODE END ETH_IRQn 1 */ } +/** + * @brief This function handles Ethernet wake-up interrupt through EXTI line 86. + */ +void ETH_WKUP_IRQHandler(void) +{ + /* USER CODE BEGIN ETH_WKUP_IRQn 0 */ + + /* USER CODE END ETH_WKUP_IRQn 0 */ + HAL_ETH_IRQHandler(&heth); + /* USER CODE BEGIN ETH_WKUP_IRQn 1 */ + + /* USER CODE END ETH_WKUP_IRQn 1 */ +} + /** * @brief This function handles USART6 global interrupt. */ diff --git a/Core/Src/tim.c b/Core/Src/tim.c index 13898d2..1b5d070 100644 --- a/Core/Src/tim.c +++ b/Core/Src/tim.c @@ -124,7 +124,7 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle) __HAL_RCC_TIM1_CLK_ENABLE(); /* TIM1 interrupt Init */ - HAL_NVIC_SetPriority(TIM1_UP_IRQn, 0, 0); + HAL_NVIC_SetPriority(TIM1_UP_IRQn, 1, 0); HAL_NVIC_EnableIRQ(TIM1_UP_IRQn); /* USER CODE BEGIN TIM1_MspInit 1 */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index 0abb9d8..198528d 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -464,7 +464,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_lpuart1_tx); /* LPUART1 interrupt Init */ - HAL_NVIC_SetPriority(LPUART1_IRQn, 0, 0); + HAL_NVIC_SetPriority(LPUART1_IRQn, 4, 0); HAL_NVIC_EnableIRQ(LPUART1_IRQn); /* USER CODE BEGIN LPUART1_MspInit 1 */ @@ -523,7 +523,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_uart4_tx); /* UART4 interrupt Init */ - HAL_NVIC_SetPriority(UART4_IRQn, 0, 0); + HAL_NVIC_SetPriority(UART4_IRQn, 4, 0); HAL_NVIC_EnableIRQ(UART4_IRQn); /* USER CODE BEGIN UART4_MspInit 1 */ @@ -587,7 +587,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_uart5_tx); /* UART5 interrupt Init */ - HAL_NVIC_SetPriority(UART5_IRQn, 0, 0); + HAL_NVIC_SetPriority(UART5_IRQn, 4, 0); HAL_NVIC_EnableIRQ(UART5_IRQn); /* USER CODE BEGIN UART5_MspInit 1 */ @@ -643,7 +643,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_uart7_tx); /* UART7 interrupt Init */ - HAL_NVIC_SetPriority(UART7_IRQn, 0, 0); + HAL_NVIC_SetPriority(UART7_IRQn, 4, 0); HAL_NVIC_EnableIRQ(UART7_IRQn); /* USER CODE BEGIN UART7_MspInit 1 */ @@ -702,7 +702,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx); /* USART1 interrupt Init */ - HAL_NVIC_SetPriority(USART1_IRQn, 0, 0); + HAL_NVIC_SetPriority(USART1_IRQn, 4, 0); HAL_NVIC_EnableIRQ(USART1_IRQn); /* USER CODE BEGIN USART1_MspInit 1 */ @@ -758,7 +758,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart2_tx); /* USART2 interrupt Init */ - HAL_NVIC_SetPriority(USART2_IRQn, 0, 0); + HAL_NVIC_SetPriority(USART2_IRQn, 4, 0); HAL_NVIC_EnableIRQ(USART2_IRQn); /* USER CODE BEGIN USART2_MspInit 1 */ @@ -814,7 +814,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart3_tx); /* USART3 interrupt Init */ - HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); + HAL_NVIC_SetPriority(USART3_IRQn, 4, 0); HAL_NVIC_EnableIRQ(USART3_IRQn); /* USER CODE BEGIN USART3_MspInit 1 */ @@ -870,7 +870,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart6_tx); /* USART6 interrupt Init */ - HAL_NVIC_SetPriority(USART6_IRQn, 0, 0); + HAL_NVIC_SetPriority(USART6_IRQn, 4, 0); HAL_NVIC_EnableIRQ(USART6_IRQn); /* USER CODE BEGIN USART6_MspInit 1 */ diff --git a/GP_Floor_Roughening Debug.launch b/GP_Floor_Roughening Debug.launch new file mode 100644 index 0000000..c9712b6 --- /dev/null +++ b/GP_Floor_Roughening Debug.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Roughening_UDPV2_0BB_WiredRPM.ioc b/GP_Floor_Roughening.ioc similarity index 95% rename from Roughening_UDPV2_0BB_WiredRPM.ioc rename to GP_Floor_Roughening.ioc index 352e639..8ae61e1 100644 --- a/Roughening_UDPV2_0BB_WiredRPM.ioc +++ b/GP_Floor_Roughening.ioc @@ -370,38 +370,39 @@ MxCube.Version=6.6.1 MxDb.Version=DB.6.0.60 NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true -NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream0_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream2_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:3\:0\:true\:false\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.ETH_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true -NVIC.FDCAN1_IT0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.FDCAN2_IT0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.ETH_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true +NVIC.ETH_WKUP_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true +NVIC.FDCAN1_IT0_IRQn=true\:2\:0\:true\:false\:true\:true\:true\:true +NVIC.FDCAN2_IT0_IRQn=true\:2\:0\:true\:false\:true\:true\:true\:true NVIC.ForceEnableDMAVector=true NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.I2C4_ER_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.I2C4_EV_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.LPUART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.I2C4_ER_IRQn=true\:15\:0\:true\:false\:true\:true\:true\:true +NVIC.I2C4_EV_IRQn=true\:15\:0\:true\:false\:true\:true\:true\:true +NVIC.LPUART1_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 NVIC.QUADSPI_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false -NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false -NVIC.TIM1_UP_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.SysTick_IRQn=true\:0\:0\:true\:false\:true\:false\:true\:false +NVIC.TIM1_UP_IRQn=true\:1\:0\:true\:false\:true\:true\:true\:true NVIC.TIM8_UP_TIM13_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.UART4_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.UART5_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.UART7_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.USART3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true -NVIC.USART6_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.UART4_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.UART5_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.UART7_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.USART1_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.USART2_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.USART3_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true +NVIC.USART6_IRQn=true\:4\:0\:true\:false\:true\:true\:true\:true NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false PA0.GPIOParameters=PinState,GPIO_Label PA0.GPIO_Label=OUT_2 @@ -685,8 +686,8 @@ ProjectManager.MainLocation=Core/Src ProjectManager.NoMain=false ProjectManager.PreviousToolchain=STM32CubeIDE ProjectManager.ProjectBuild=false -ProjectManager.ProjectFileName=Roughening_UDPV2_0BB_Wired.ioc -ProjectManager.ProjectName=Roughening_UDPV2_0BB_Wired +ProjectManager.ProjectFileName=Roughening_UDPV2_0BB_WiredRPM.ioc +ProjectManager.ProjectName=Roughening_UDPV2_0BB_WiredRPM ProjectManager.RegisterCallBack= ProjectManager.StackSize=0x1000 ProjectManager.TargetToolchain=STM32CubeIDE diff --git a/LWIP/Target/ethernetif.c b/LWIP/Target/ethernetif.c index 2fda414..057c298 100644 --- a/LWIP/Target/ethernetif.c +++ b/LWIP/Target/ethernetif.c @@ -525,8 +525,10 @@ void HAL_ETH_MspInit(ETH_HandleTypeDef* ethHandle) HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); /* Peripheral interrupt init */ - HAL_NVIC_SetPriority(ETH_IRQn, 6, 0); + HAL_NVIC_SetPriority(ETH_IRQn, 5, 0); HAL_NVIC_EnableIRQ(ETH_IRQn); + HAL_NVIC_SetPriority(ETH_WKUP_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(ETH_WKUP_IRQn); /* USER CODE BEGIN ETH_MspInit 1 */ /* USER CODE END ETH_MspInit 1 */ @@ -565,6 +567,8 @@ void HAL_ETH_MspDeInit(ETH_HandleTypeDef* ethHandle) /* Peripheral interrupt Deinit*/ HAL_NVIC_DisableIRQ(ETH_IRQn); + HAL_NVIC_DisableIRQ(ETH_WKUP_IRQn); + /* USER CODE BEGIN ETH_MspDeInit 1 */ /* USER CODE END ETH_MspDeInit 1 */ diff --git a/Roughening_UDPV3_0BB_WiredRPM Debug.launch b/Roughening_UDPV3_0BB_WiredRPM Debug.launch new file mode 100644 index 0000000..07e9ea4 --- /dev/null +++ b/Roughening_UDPV3_0BB_WiredRPM Debug.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/readme.txt b/readme.txt index 0850643..e127da0 100644 --- a/readme.txt +++ b/readme.txt @@ -1 +1,4 @@ 25/12/26 加UDP + +V3： +260423：新增右摇杆压力模式区分