XiaoPengfei
/
32_Roughening_BigBoard
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

first commit

master
LAPTOPNUM\lapto 2 months ago
parent
256fc82364
commit
d855954639
29 changed files with 1192 additions and 414 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .project
  2. 4
      .settings/language.settings.xml
  3. 2
      Core/BASE/Protobuf/PSource/bsp_GV.pb.h
  4. 17
      Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h
  5. 3
      Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto
  6. 4
      Core/BASE/Src/BSP/bsp_MB_host.c
  7. 5
      Core/BASE/Src/MSP/msp_ground_management.c
  8. 123
      Core/BASE/Src/MSP/msp_strain_gauge.c
  9. 1
      Core/FSM/Inc/fsm_state_control.h
  10. 9
      Core/FSM/Inc/robot_move_actions.h
  11. 85
      Core/FSM/Src/change_line_control.c
  12. 520
      Core/FSM/Src/fsm_state_control.c
  13. 257
      Core/FSM/Src/motors.c
  14. 68
      Core/FSM/Src/robot_move_actions.c
  15. 1
      Core/Inc/main.h
  16. 2
      Core/Inc/stm32h7xx_hal_conf.h
  17. 1
      Core/Inc/stm32h7xx_it.h
  18. 14
      Core/Src/dma.c
  19. 4
      Core/Src/fdcan.c
  20. 4
      Core/Src/i2c.c
  21. 6
      Core/Src/main.c
  22. 14
      Core/Src/stm32h7xx_it.c
  23. 2
      Core/Src/tim.c
  24. 16
      Core/Src/usart.c
  25. 80
      GP_Floor_Roughening Debug.launch
  26. 49
      GP_Floor_Roughening.ioc
  27. 6
      LWIP/Target/ethernetif.c
  28. 80
      Roughening_UDPV3_0BB_WiredRPM Debug.launch
  29. 3
      readme.txt

2
.project
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>Roughening_UDPV2_0BB_WiredRPM</name>
<name>GP_Floor_Roughening</name>
<comment></comment>
<projects>
</projects>

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1377807682938255891" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1274559451698783667" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1377807682938255891" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1274559451698783667" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>

2
Core/BASE/Protobuf/PSource/bsp_GV.pb.h
View File

@ -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" */

17
Core/BASE/Protobuf/PSource/bsp_strain_gauge.pb.h
View File

@ -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" */

3
Core/BASE/Protobuf/Proto/bsp_strain_gauge.proto
View File

@ -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;
};

4
Core/BASE/Src/BSP/bsp_MB_host.c
View File

@ -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++)

5
Core/BASE/Src/MSP/msp_ground_management.c
View File

@ -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);

123
Core/BASE/Src/MSP/msp_strain_gauge.c
View File

@ -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;
@ -46,15 +51,16 @@ 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 后退
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**/
@ -63,32 +69,61 @@ void strain_gauge_loop()
/*写寄存器2 K**/
MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf,
&strain_gauge_handler->TxCount, strain_gauge_slave_id, 2,
strainGaugeValue->HX711_K);
(uint16_t) strainGaugeValue->HX711_K);
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);
/*写寄存器3 D**/
MB_WriteHoldingReg(&strain_gauge_handler->Tx_Buf,
&strain_gauge_handler->TxCount, strain_gauge_slave_id, 3,
strainGaugeValue->HX711_D);
/*写寄存器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,
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, decode_strain_gauge_56);
SAVE_Register23 = 0;
}
/*写寄存器9 设置为55保存KD**/
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->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);
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 ;
}
}
int16_t decoded_strain_gauge_holdingReg_value[20];
@ -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)
{

1
Core/FSM/Inc/fsm_state_control.h
View File

@ -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();

9
Core/FSM/Inc/robot_move_actions.h
View File

@ -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_ */

85
Core/FSM/Src/change_line_control.c
View File

@ -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(&current_robot_move_state,
&robot_move_head_to_right_enum_state); /*转到朝上*/
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_right_enum_state); /*转到朝上*/
fsm_state_set(&current_robot_move_state, &robot_move_head_to_right_add_adjust_enum_state); /*转到朝右+竖直微调项*/
}
break;
}
case HorizontalChange_TurnToRight:
{
fsm_state_set(&current_robot_move_state,
&robot_move_head_to_right_enum_state); /*转到朝右*/
if (abs(GV.Robot_Angle - CV.RobotRightAngleValue)
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_right_enum_state); /*转到朝右*/
fsm_state_set(&current_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(&current_robot_move_state,
&robot_move_head_to_left_enum_state); /*转到朝左*/
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_left_enum_state); /*转到朝左*/
fsm_state_set(&current_robot_move_state, &robot_move_head_to_left_add_adjust_enum_state); /*转到朝左+竖直微调项*/
}
break;
}
case HorizontalChange_TurnToLeft:
{
fsm_state_set(&current_robot_move_state,
&robot_move_head_to_left_enum_state); /*转到朝左*/
if (abs(GV.Robot_Angle - CV.RobotLeftAngleValue)
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_left_enum_state); /*转到朝左*/
fsm_state_set(&current_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(&current_robot_move_state,
&robot_move_head_to_up_enum_state); /* 移动至头朝上 */
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
fsm_state_set(&current_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(&current_robot_move_state,
&robot_move_head_to_up_enum_state); /* 移动至头朝上 */
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
fsm_state_set(&current_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(&current_robot_move_state,
&robot_move_head_to_up_enum_state); /* 移动至头朝上 */
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
fsm_state_set(&current_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(&current_robot_move_state,
&robot_move_head_to_up_enum_state); /* 移动至头朝上 */
// fsm_state_set(&current_robot_move_state,
// &robot_move_head_to_up_enum_state); /* 移动至头朝上 */
fsm_state_set(&current_robot_move_state, &robot_move_head_to_up_add_adjust_enum_state); /* 移动至头朝上+竖直微调 */
}
else
{

520
Core/FSM/Src/fsm_state_control.c
View File

@ -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(&current_robot_move_state, &robot_halt_state);
@ -102,8 +105,13 @@ void Fsm_Init()
//机器人电机供电状态初始化
fsm_state_init(&current_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(&current_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(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_roughening_state, &roughening_halt_state);
// // ============================================================
// // 【配置参数】
// // ============================================================
// const int COUNT_LIMIT = 250; // 0.5s 时间滤波
//
// if (IV.Press >= 500) {
// SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert);
// g_IsPressureLocked = 1; // 进入过载锁定状态
// }
// if (MotorErrorDetect() == 1) {
// 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. 压力检测与锁定逻辑
// // ============================================================
//
// // 使用全局变量：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
// {
// SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert);
// // 增加迟滞，防止在500附近频繁切换，比如低于480才解除
// if (g_IsPressureLocked == 1 && IV.Press < 480) {
// g_IsPressureLocked = 0; // 解除锁定，恢复正常
// 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;
// }
// }
// else
// {
// pressure_over_count = 0;
// }
// }
//
// // 电机错误（原注释代码保持不变）
// // if (MotorErrorDetect() == 1)
// // {
// // fsm_state_set(&current_robot_move_state, &robot_halt_state);
// // fsm_state_set(&current_roughening_state, &roughening_halt_state);
// // g_IsPressureLocked = 1;
// // }
//
// // ... (补偿控制和速度计算代码保持不变) ...
// // 报警位处理
// if (g_IsPressureLocked == 1)
// {
// SET_BIT_1(SystemErrorCode, ComError_Pressure_Detect_Alert);
// fsm_state_set(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_roughening_state, &roughening_halt_state);
// }
// else
// {
// SET_BIT_0(SystemErrorCode, ComError_Pressure_Detect_Alert);
// }
//
// // 补偿与速度计算（保持不变）
// Lcompensation_control();
// Rcompensation_control();
// speed_selection = 2.0 * (P_MK32->CH11_RD1 + 1000) / 200;
@ -247,35 +322,56 @@ 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(&current_tilt_state, &tilt_halt_state);
// }
// else
// {
// // 正常工作状态：调用正常的摇杆控制
// Mannual_TiltControl();
// needs_restricted_mode = false;
// }
//
// // ============================================================
// // 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回
// // 3. 执行对应的摇杆控制函数
// // ============================================================
// if (g_IsPressureLocked == 1)
// if (needs_restricted_mode)
// {
// // 再次确保状态机处于停止态（双重保险）
// fsm_state_set(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_roughening_state, &roughening_halt_state);
// // 此时 current_tilt_state 已经是 tilt_halt_state 了
// // Mannual_TiltControl1 将在这个“停止”的基础上运行
// // 逻辑是：检测到下压角度 -> 忽略/保持停止；检测到上抬角度 -> 允许上抬
// // Mannual_TiltControl1();
//
// // 直接退出，不执行后面的自动巡航、换道、手动行走等代码
// return;
// // 更改为压力自动恢复正常值:回归到屏幕设定的压力值内
// Mannual_TiltControl2();
// }
// else
// {
// // 正常模式，无干预
// Mannual_TiltControl();
// }
//
// // --- 以下代码只有在未锁定 (g_IsPressureLocked == 0) 时才会执行 ---
// // ============================================================
// // 4. 【运动互锁】：如果锁定，禁止机器人和拉毛盘运动，直接返回。暂时关闭这个选项，因为有自适应压力调节，所以不需要锁定机器人运动
// // ============================================================
//// if (g_IsPressureLocked == 1)
//// {
// // 暂时关闭这个选项，因为有自适应压力调节，所以不需要锁定机器人运动
//// fsm_state_set(&current_robot_move_state, &robot_halt_state);
//// fsm_state_set(&current_roughening_state, &roughening_halt_state);
//// return;
//// }
//
// DHRougheningControl(); /* 拉毛盘控制 */
// // ============================================================
// // 5. 正常运动逻辑
// // ============================================================
// DHRougheningControl();
//
// if (GV.PV.RunMode == Move_Automation_Move_Horizontal_Move)
// {
@ -299,63 +395,140 @@ void GF_Dispatch()
// 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; // 震荡确认次数
static int pressure_over_count = 0;
bool needs_restricted_mode = false; // 标记是否需要受限模式
// 动态计算滞回区间：即三级压力预警的剩余两个值
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. 压力检测与锁定逻辑
// 1. 降采样震荡检测逻辑
// ============================================================
sample_timer++;
if (IV.Press >= PRESSURE_LOCK_THRESHOLD)
// 只有当计时器达到设定步长时，才进行一次“有效对比”
if (sample_timer >= g_Pressure_Oscillation_Step)
{
pressure_over_count++;
if (pressure_over_count >= COUNT_LIMIT)
// int diff = current_press - last_sampled_pressure;
diff = current_press - last_sampled_pressure;
if (diff < 0) diff = -diff; // 取绝对值
// 判断是否剧烈跳变
if (diff >= OSCILLATION_THRESHOLD)
{
g_IsPressureLocked = 1;
if (pressure_over_count > COUNT_LIMIT + 50) pressure_over_count = COUNT_LIMIT + 50;
}
oscillation_count++;
}
else
{
// 波动恢复正常，震荡计数清零
oscillation_count = 0;
}
last_sampled_pressure = current_press;
sample_timer = 0;
}
// ============================================================
// 2. 状态机逻辑 (修复解锁漏洞)
// ============================================================
// --- 情况 A：如果当前已经锁定 ---
if (g_IsPressureLocked == 1)
{
if (IV.Press < PRESSURE_UNLOCK_THRESHOLD)
// 判断是否可以解锁
// 逻辑：必须同时满足两个条件才算安全
// 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;
}
else
// 否则保持锁定状态，直接返回，不再执行下面的超压计数逻辑
return;
}
// --- 情况 B：当前未锁定，检查是否需要触发锁定 ---
// 1. 优先检查震荡 (震荡优先级高于超压)
if (oscillation_count >= OSCILLATION_CONFIRM_COUNT)
{
// 保持在锁定状态，压力在 4500-5000 之间
pressure_over_count = 0;
g_IsPressureLocked = 1;
return; // 锁定后直接返回
}
// 2. 检查持续超压
if (current_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
@ -364,15 +537,19 @@ void MoveControl()
}
}
// 电机错误
// if (MotorErrorDetect() == 1)
// {
// fsm_state_set(&current_robot_move_state, &robot_halt_state);
// fsm_state_set(&current_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(&current_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(&current_robot_move_state, &robot_halt_state);
fsm_state_set(&current_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,107 +772,112 @@ 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(&current_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(&current_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(&current_tilt_state, &tilt_down_state);
// return;
// }
// fsm_state_set(&current_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)/*停止*//*600*/
&& abs(P_MK32->CH0_RY_H) <= CV.Joy_Sticker_Value_Allowance)
{
fsm_state_set(&current_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(&current_tilt_state, &tilt_up_state);/*上升*/
return;
}
// 原下压逻辑：无限位
if (abs(angle - 90) <= CV.Joy_Sticker_Angle_Allowance)/*45° 下降*/
// 下降
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(&current_tilt_state, &tilt_down_state);
}
else
{
fsm_state_set(&current_tilt_state, &tilt_halt_state);
}
}
else
{
// 情况2: 预设值不为0，即用户有输入，能压到哪里受限于屏幕预设值
if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet)
{
fsm_state_set(&current_tilt_state, &tilt_down_state);
return;
}
else
{
fsm_state_set(&current_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)
// {
// int current_pressure = IV.Press;
//
// // --- 新增软限位 (带消抖) ---
// 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(&current_tilt_state, &tilt_halt_state);
// // 此时计数器保持在高位，直到压力降低或摇杆回中才清零
// }
// else
// {
// // 处于消抖过程中 (例如超压了 50ms)，视为干扰，允许继续下降
// fsm_state_set(&current_tilt_state, &tilt_down_state);
// }
// }
// else
// {
// // 压力正常， 计数器清零 (打破连续性)
// pressure_over_count = 0;
//
// // 允许下降
// fsm_state_set(&current_tilt_state, &tilt_down_state);
// }
//
// return;
// }
//
// // 其他角度情况，默认停止
// fsm_state_set(&current_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 (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(&current_tilt_state, &tilt_halt_state); /*停止推杆*/
return;
}
int angle = atan2(P_MK32->CH1_RY_V, P_MK32->CH0_RY_H) * 180 / M_PI;
// const int g_AUTO_LIFT_RELEASE_PRESSURE = 1500; // 自动抬升的停止阈值
if (abs(angle - (-90)) <= CV.Joy_Sticker_Angle_Allowance)
// 1. 自动抬升逻辑
// 只要压力还大于停止阈值，就一直保持抬升状态
if (IV.Press > g_AUTO_LIFT_RELEASE_PRESSURE)
{
fsm_state_set(&current_tilt_state, &tilt_up_state);/*上升*/
fsm_state_set(&current_tilt_state, &tilt_up_state); /*强制上升*/
return;
}
}
// 2. 停止逻辑
// 当压力降到停止阈值以下，停止推杆
fsm_state_set(&current_tilt_state, &tilt_halt_state); /*停止推杆*/
}
int Auto_TiltControl()
@ -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(&current_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(&current_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(&current_tilt_state, &tilt_up_state); /*上升推杆*/
// return 1;
//
// }
else if (GV.Strain_Gauge.Pressure <= GV.PV.PressSet * 0.8)
{
fsm_state_set(&current_tilt_state, &tilt_down_state); /*下降推杆*/
return 1;
@ -904,6 +1062,16 @@ int AbnormalDetect()
fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
return 1;
}
// 预留：初次上电时，检测压力传感器值是否在零位附近如【-50，+50】
// if (IV.Press >= -50 && IV.Press <= 50)
// {
//// fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/
// fsm_state_set(&current_roughening_state, &roughening_halt_state); /*关闭拉毛盘*/
// return 1;
// }
if (P_MK32->IsOnline == 0) //等于0时 subus有数，但是遥控器关机了，或者失联
{
fsm_state_set(&current_robot_move_state, &robot_halt_state); /*停止机器人*/

257
Core/FSM/Src/motors.c
View File

@ -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,17 +147,57 @@ 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);
}
void Roughening_Motor_Controller_intialize(FDCANHandler *Handler)
{
Roughening_Motor_Controller = Handler;
Roughening_Motor_Controller->CAN_Decode = Roughening_MotorDecodeCAN;
@ -38,27 +212,52 @@ void Roughening_Motor_Controller_intialize(FDCANHandler *Handler)
Roughening_DispacherController, MotorCommandsLoop);
LOGFF(DL_WARN,"TT_Motors_intialize");
}
void MotorCommandsLoop()
{
static int heartbeat_counter = 0;
if (TT_Motor_Need_To_Activate == 1)
{
ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000);
// 1. 激活电机 (内部可能包含状态机切换)
ActivateMotor(LeftMotorID, Roughening_Motor_Controller, 2000);
ActivateMotor(RightMotorID, Roughening_Motor_Controller, 2000);
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);
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);
@ -67,23 +266,33 @@ void MotorCommandsLoop()
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,
@ -99,14 +308,28 @@ void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
}
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));
}

68
Core/FSM/Src/robot_move_actions.c
View File

@ -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 ;
}
}

1
Core/Inc/main.h
View File

@ -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

2
Core/Inc/stm32h7xx_hal_conf.h
View File

@ -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 */

1
Core/Inc/stm32h7xx_it.h
View File

@ -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);

14
Core/Src/dma.c
View File

@ -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);
}

4
Core/Src/fdcan.c
View File

@ -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 */

4
Core/Src/i2c.c
View File

@ -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 */

6
Core/Src/main.c
View File

@ -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,7 +115,6 @@ int main(void)
/* USER CODE END 1 */
/* MPU Configuration--------------------------------------------------------*/
MPU_Config();
/* Enable I-Cache---------------------------------------------------------*/
@ -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);

14
Core/Src/stm32h7xx_it.c
View File

@ -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.
*/

2
Core/Src/tim.c
View File

@ -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 */

16
Core/Src/usart.c
View File

@ -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 */

80
GP_Floor_Roughening Debug.launch
View File

@ -0,0 +1,80 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<launchConfiguration type="com.st.stm32cube.ide.mcu.debug.launch.launchConfigurationType">
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false"/>
<intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.ip_address_local" value="localhost"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.enabled" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.value" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.loadList" value="{&quot;fItems&quot;:[{&quot;fIsFromMainTab&quot;:true,&quot;fPath&quot;:&quot;Debug/GP_Floor_Roughening.elf&quot;,&quot;fProjectName&quot;:&quot;GP_Floor_Roughening&quot;,&quot;fPerformBuild&quot;:true,&quot;fDownload&quot;:true,&quot;fLoadSymbols&quot;:true}]}"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.override_start_address_mode" value="default"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.remoteCommand" value="target remote"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startServer" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.divby0" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.unaligned" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.haltonexception" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swd_mode" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_port" value="61235"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_trace_hclk" value="16000000"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_external_loader" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader_init" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="C:\Users\lapto\Desktop\BHBF_robot\01_Lamao_ROBOT_Source\GP_Floor_Roughening_V1_2605\Debug\st-link_gdbserver_log.txt"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="enable"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_check_serial_number" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverAuto" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverPort" value="cortex_m0"/>
<intAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyPort" value="60000"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doHalt" value="false"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doReset" value="false"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value=""/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDeviceId" value="com.st.stm32cube.ide.mcu.debug.stlink"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.pcRegister" value=""/>
<intAttribute key="org.eclipse.cdt.debug.gdbjtag.core.portNumber" value="61234"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.runCommands" value=""/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="true"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
<stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/>
<intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
<stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="remote"/>
<booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true"/>
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main"/>
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/GP_Floor_Roughening.elf"/>
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="GP_Floor_Roughening"/>
<booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true"/>
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.456396271"/>
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
<listEntry value="/GP_Floor_Roughening"/>
</listAttribute>
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
<listEntry value="4"/>
</listAttribute>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;"/>
<stringAttribute key="process_factory_id" value="com.st.stm32cube.ide.mcu.debug.launch.HardwareDebugProcessFactory"/>
</launchConfiguration>

49
Roughening_UDPV2_0BB_WiredRPM.ioc → GP_Floor_Roughening.ioc
View File

@ -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

6
LWIP/Target/ethernetif.c
View File

@ -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 */

80
Roughening_UDPV3_0BB_WiredRPM Debug.launch
View File

@ -0,0 +1,80 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<launchConfiguration type="com.st.stm32cube.ide.mcu.debug.launch.launchConfigurationType">
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.access_port_id" value="0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_live_expr" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.enable_swv" value="false"/>
<intAttribute key="com.st.stm32cube.ide.mcu.debug.launch.formatVersion" value="2"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.ip_address_local" value="localhost"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.enabled" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.limit_swo_clock.value" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.loadList" value="{&quot;fItems&quot;:[{&quot;fIsFromMainTab&quot;:true,&quot;fPath&quot;:&quot;Debug/Roughening_UDPV3_0BB_WiredRPM.elf&quot;,&quot;fProjectName&quot;:&quot;Roughening_UDPV3_0BB_WiredRPM&quot;,&quot;fPerformBuild&quot;:true,&quot;fDownload&quot;:true,&quot;fLoadSymbols&quot;:true}]}"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.override_start_address_mode" value="default"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.remoteCommand" value="target remote"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startServer" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.divby0" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.exception.unaligned" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.startuptab.haltonexception" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swd_mode" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_port" value="61235"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.swv_trace_hclk" value="16000000"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.useRemoteTarget" value="true"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.launch.vector_table" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.launch.verify_flash_download" value="true"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_allow_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.cti_signal_halt" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_external_loader" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_logging" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_max_halt_delay" value="false"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.enable_shared_stlink" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader" value=""/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.external_loader_init" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.frequency" value="0"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.halt_all_on_reset" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.log_file" value="C:\Users\lapto\Desktop\BHBF_robot\01_Lamao_ROBOT_Source\Roughening_UDPV3_0BB_WiredRPM\Debug\st-link_gdbserver_log.txt"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.low_power_debug" value="enable"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_check_serial_number" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>
<booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverAuto" value="false"/>
<stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriverPort" value="cortex_m0"/>
<intAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyPort" value="60000"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doHalt" value="false"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doReset" value="false"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value=""/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDeviceId" value="com.st.stm32cube.ide.mcu.debug.stlink"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.pcRegister" value=""/>
<intAttribute key="org.eclipse.cdt.debug.gdbjtag.core.portNumber" value="61234"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.runCommands" value=""/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="true"/>
<booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
<stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="arm-none-eabi-gdb"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false"/>
<booleanAttribute key="org.eclipse.cdt.dsf.gdb.UPDATE_THREADLIST_ON_SUSPEND" value="false"/>
<intAttribute key="org.eclipse.cdt.launch.ATTR_BUILD_BEFORE_LAUNCH_ATTR" value="2"/>
<stringAttribute key="org.eclipse.cdt.launch.COREFILE_PATH" value=""/>
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="remote"/>
<booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true"/>
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main"/>
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/Roughening_UDPV3_0BB_WiredRPM.elf"/>
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="Roughening_UDPV3_0BB_WiredRPM"/>
<booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true"/>
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.456396271"/>
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
<listEntry value="/Roughening_UDPV3_0BB_WiredRPM"/>
</listAttribute>
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
<listEntry value="4"/>
</listAttribute>
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;"/>
<stringAttribute key="process_factory_id" value="com.st.stm32cube.ide.mcu.debug.launch.HardwareDebugProcessFactory"/>
</launchConfiguration>

3
readme.txt
View File

@ -1 +1,4 @@
25/12/26 加UDP
V3：
260423：新增右摇杆压力模式区分

Write Preview
Loading…
Cancel
Save