YuLin_Zhen
/
FW3.0
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity
Browse Source

新增：配合界面增加竖直左右换道；日志；

master FW3.0版本10.10
LIN\54376 4 weeks ago
parent
50c3cd6f3b
commit
27f6b7dfea
20 changed files with 327 additions and 201 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. 1
      .settings/language.settings.xml
  2. 3
      BASE/Inc/MSP/msp_U7.h
  3. 4
      BASE/Src/BSP/bsp_decode_command.c
  4. 73
      BASE/Src/MSP/msp_U7.c
  5. 9
      Core/Inc/FSM.h
  6. 8
      Core/Inc/robot_state.h
  7. 2
      Core/Protobuf/PSource/bsp_Cmd.pb.c
  8. 13
      Core/Protobuf/PSource/bsp_Cmd.pb.h
  9. 12
      Core/Protobuf/PSource/bsp_Error.pb.h
  10. 2
      Core/Protobuf/PSource/bsp_GV.pb.h
  11. 17
      Core/Protobuf/PSource/bsp_IV.pb.h
  12. 11
      Core/Protobuf/PSource/bsp_PV.pb.h
  13. 2
      Core/Protobuf/PSource/bsp_ReCmd.pb.c
  14. 13
      Core/Protobuf/PSource/bsp_ReCmd.pb.h
  15. 1
      Core/Protobuf/Proto/bsp_GV.proto
  16. 5
      Core/Protobuf/Proto/bsp_IV.proto
  17. 3
      Core/Protobuf/Proto/bsp_PV.proto
  18. 18
      Core/Src/FSM.c
  19. 3
      Core/Src/main.c
  20. 208
      Core/Src/robot_state.c

1
.settings/language.settings.xml
View File

@ -16,7 +16,6 @@
<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 copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1328381156250390778" 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++"/>

3
BASE/Inc/MSP/msp_U7.h
View File

@ -11,6 +11,9 @@
#include "msp_U7.pb.h"
void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value);
extern SP_MSP_U7_Button *P_U7;
void Robot_ResetCheck_Loop(void);
//
extern void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler);
uint8_t IsRemoteAtInitialPosition(void);
#endif /* INC_MSP_MSP_U7_H_ */

4
BASE/Src/BSP/bsp_decode_command.c
View File

@ -269,7 +269,7 @@ void decode_command_and_feedback(uint8_t *buffer, uint16_t length, char isMqtt,
pb_istream_t i_cv_stream =
{ 0 };
i_cv_stream = pb_istream_from_buffer(decoded_Cmd.Buff_Data,
i_cv_stream = pb_istream_from_buffer(&decoded_Cmd.Buff_Data,
decoded_Cmd.Buff_Data_Length);
pb_decode(&i_cv_stream, CV_struct_define_fields, &decoded_CV);
//将CV写入EEPROM
@ -320,7 +320,7 @@ void WrapInCmdAndSend(ReCmd send_Cmd, uint8_t *buf, char isMqtt,
struct UARTHandler *send_Handler)
{
memcpy(send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length);
memcpy(&send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length);
pb_ostream_t ReCmd_out_stream =
{ 0 };

73
BASE/Src/MSP/msp_U7.c
View File

@ -6,30 +6,66 @@
*/
#include "msp_U7.h"
#include "DLT/DLTuc.h"
#include "BHBF_ROBOT.h"
#include "fsm.h"
#include "robot_state.h"
uint8_t Reset_Flag = 0;
SP_MSP_U7_Button *P_U7;
struct UARTHandler *U7_Sbus_Controller;
void decode_U7Data(uint8_t *buffer, uint16_t length)
{
if(length!=25)
if(length != 25)
{
return;
}
if(buffer[0]==0X0f&&buffer[24]==0X00)
if(buffer[0] == 0x0F && buffer[24] == 0x00)
{
Sbus_Data_Count_U7(&buffer[1], (int32_t*) (P_U7));
Sbus_Data_Count_U7(&buffer[1], (int32_t*)(P_U7));
// =================== 开机复位检测 ===================
if (Reset_Flag == 0) // 第一次收到数据
{
Reset_Flag = 1;
if(!IsRemoteAtInitialPosition()) // 如果未复位
{
system_mode = SYSTEM_RESET;
CurrentMoveState = Move_Reset;
HALT_State_Do(); // 立即停车并清空状态
}
else
{
system_mode = SYSTEM_NORMAL; // 正常运行
}
}
else
{
// =================== 后续检测 ===================
if(system_mode == SYSTEM_RESET)
{
if(IsRemoteAtInitialPosition()) // 按键已复位
{
system_mode = SYSTEM_NORMAL;
CurrentMoveState = Move_Resetted;
LOG("U7 remote reset recovered, resume NORMAL mode");
}
else
{
// 还没复位，保持 RESET
HALT_State_Do();
}
}
}
}
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"U7_sbus",1);
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "U7_sbus", 1);
}
void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value)
{
@ -85,9 +121,28 @@ void Sbus_Data_Count_U7(uint8_t *buf, int32_t *But_Value)
void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler)
{
U7_Sbus_Controller = Handler;
U7_Sbus_Controller->UART_Decode=decode_U7Data;
U7_Sbus_Controller->UART_Decode = decode_U7Data;
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"U7_sbus",0,u7_sbus);
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController, "U7_sbus", 0, u7_sbus);
LOG("U7_Sbus_intialize");
}
uint8_t IsRemoteAtInitialPosition(void)
{
if(GV.U7_Key.CH0_LYB_V != 0) return 0;
if(GV.U7_Key.CH1_LYB_H != 0) return 0;
if(GV.U7_Key.LU_Single != -1000) return 0;
if(GV.U7_Key.LU_Three != 0) return 0;
if(GV.U7_Key.LU_pulley != 0) return 0;
if(GV.U7_Key.M1 != -1000) return 0;
if(GV.U7_Key.M2 != -1000) return 0;
if(GV.U7_Key.M3 != -1000) return 0;
if(GV.U7_Key.M5 != -1000) return 0;
if(GV.U7_Key.M6 != -1000) return 0;
if(GV.U7_Key.RU_Single != -1000) return 0;
if(GV.U7_Key.RU_Three != 0) return 0;
LOG("U7_At_Initial_Posotion");
return 1; // 全部归零
}

9
Core/Inc/FSM.h
View File

@ -55,10 +55,12 @@ typedef enum _MoveSTATE_t
Move_ChgFinish,
Move_Emergency,
Move_Reset,
Move_Resetted,
} MoveSTATE_t;
extern MoveSTATE_t CurrentMoveState;
//设置 换道距离和设置后退距离
typedef enum _SetSTATE_t
{
@ -93,11 +95,12 @@ typedef struct _transition_t
void (*robotRun)(void); //执行相关动作
} transition_t;
extern uint8_t Reset_Flag;
extern int32_t* RobotSpeed;
extern int32_t JontSwingSpeed;
extern int32_t JontTiltSpeed;
extern int Polish_Flag;
extern int Vacuum_Flag;
//extern int Vacuum_Flag;
extern int Spray_Flag;
void Fsm_Init();
#endif /* INC_FSM_H_ */

8
Core/Inc/robot_state.h
View File

@ -25,6 +25,7 @@ typedef struct ChgRoad_Parameters
int32_t change_road_distance; //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远）
int32_t change_road_status_flag; //换道标志位
int32_t change_road_finish_flag;
int32_t change_road_direction;
double speed;
/**转弯*/
@ -54,6 +55,13 @@ typedef struct
}AutoMove_Parameters;
typedef enum {
SYSTEM_RESET,
SYSTEM_NORMAL
} SystemMode_t;
extern SystemMode_t system_mode;
extern ChgRoad_Parameters Chg_Pa;
extern int* AuTo_Flag;

2
Core/Protobuf/PSource/bsp_Cmd.pb.c
View File

@ -6,7 +6,7 @@
#error Regenerate this file with the current version of nanopb generator.
#endif
PB_BIND(Cmd, Cmd, 2)
PB_BIND(Cmd, Cmd, AUTO)

13
Core/Protobuf/PSource/bsp_Cmd.pb.h
View File

@ -29,7 +29,7 @@ Buff_Data_Length 是下发的字节数， */
int32_t Parameter3;
int32_t Parameter4;
int32_t Buff_Data_Length;
pb_byte_t Buff_Data[512];
pb_callback_t Buff_Data;
} Cmd;
@ -38,8 +38,8 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define Cmd_init_default {0, 0, 0, 0, 0, 0, 0, {0}}
#define Cmd_init_zero {0, 0, 0, 0, 0, 0, 0, {0}}
#define Cmd_init_default {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}}
#define Cmd_init_zero {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}}
/* Field tags (for use in manual encoding/decoding) */
#define Cmd_CommadNum_tag 1
@ -60,8 +60,8 @@ X(a, STATIC, SINGULAR, INT32, Parameter2, 4) \
X(a, STATIC, SINGULAR, INT32, Parameter3, 5) \
X(a, STATIC, SINGULAR, INT32, Parameter4, 6) \
X(a, STATIC, SINGULAR, INT32, Buff_Data_Length, 7) \
X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Buff_Data, 8)
#define Cmd_CALLBACK NULL
X(a, CALLBACK, SINGULAR, BYTES, Buff_Data, 8)
#define Cmd_CALLBACK pb_default_field_callback
#define Cmd_DEFAULT NULL
extern const pb_msgdesc_t Cmd_msg;
@ -70,8 +70,7 @@ extern const pb_msgdesc_t Cmd_msg;
#define Cmd_fields &Cmd_msg
/* Maximum encoded size of messages (where known) */
#define BSP_CMD_PB_H_MAX_SIZE Cmd_size
#define Cmd_size 592
/* Cmd_size depends on runtime parameters */
#ifdef __cplusplus
} /* extern "C" */

12
Core/Protobuf/PSource/bsp_Error.pb.h
View File

@ -12,7 +12,7 @@
/* Struct definitions */
typedef struct _ErrorDataInfo {
int32_t Error_Value;
pb_byte_t Error_Name[50];
pb_callback_t Error_Name;
} ErrorDataInfo;
typedef struct _ErrorData {
@ -52,9 +52,9 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define ErrorDataInfo_init_default {0, {0}}
#define ErrorDataInfo_init_default {0, {{NULL}, NULL}}
#define ErrorData_init_default {0, 0, 0, 0, 0, 0, 0, 0}
#define ErrorDataInfo_init_zero {0, {0}}
#define ErrorDataInfo_init_zero {0, {{NULL}, NULL}}
#define ErrorData_init_zero {0, 0, 0, 0, 0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */
@ -72,8 +72,8 @@ extern "C" {
/* Struct field encoding specification for nanopb */
#define ErrorDataInfo_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, INT32, Error_Value, 1) \
X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Error_Name, 4)
#define ErrorDataInfo_CALLBACK NULL
X(a, CALLBACK, SINGULAR, BYTES, Error_Name, 4)
#define ErrorDataInfo_CALLBACK pb_default_field_callback
#define ErrorDataInfo_DEFAULT NULL
#define ErrorData_FIELDLIST(X, a) \
@ -96,8 +96,8 @@ extern const pb_msgdesc_t ErrorData_msg;
#define ErrorData_fields &ErrorData_msg
/* Maximum encoded size of messages (where known) */
/* ErrorDataInfo_size depends on runtime parameters */
#define BSP_ERROR_PB_H_MAX_SIZE ErrorData_size
#define ErrorDataInfo_size 63
#define ErrorData_size 95
#ifdef __cplusplus

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

@ -107,7 +107,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 884
#define GV_struct_define_size 895
#ifdef __cplusplus
} /* extern "C" */

17
Core/Protobuf/PSource/bsp_IV.pb.h
View File

@ -26,6 +26,9 @@ typedef struct _IV_struct_define {
int32_t Robot_Error_Right;
int32_t Robot_Compensation_Left;
int32_t Robot_Compensation_Right;
int32_t Robot_RESET;
int32_t Robot_LaneChange_Left;
int32_t Robot_LaneChange_Right;
} IV_struct_define;
@ -34,8 +37,8 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */
#define IV_struct_define_Robot_Move_AutoSpeed_tag 1
@ -52,6 +55,9 @@ extern "C" {
#define IV_struct_define_Robot_Error_Right_tag 12
#define IV_struct_define_Robot_Compensation_Left_tag 13
#define IV_struct_define_Robot_Compensation_Right_tag 14
#define IV_struct_define_Robot_RESET_tag 15
#define IV_struct_define_Robot_LaneChange_Left_tag 16
#define IV_struct_define_Robot_LaneChange_Right_tag 17
/* Struct field encoding specification for nanopb */
#define IV_struct_define_FIELDLIST(X, a) \
@ -68,7 +74,10 @@ X(a, STATIC, SINGULAR, INT32, Robot_Current_Right, 10) \
X(a, STATIC, SINGULAR, INT32, Robot_Error_Left, 11) \
X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) \
X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Left, 13) \
X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Right, 14)
X(a, STATIC, SINGULAR, INT32, Robot_Compensation_Right, 14) \
X(a, STATIC, SINGULAR, INT32, Robot_RESET, 15) \
X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Left, 16) \
X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Right, 17)
#define IV_struct_define_CALLBACK NULL
#define IV_struct_define_DEFAULT NULL
@ -79,7 +88,7 @@ extern const pb_msgdesc_t IV_struct_define_msg;
/* Maximum encoded size of messages (where known) */
#define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size
#define IV_struct_define_size 154
#define IV_struct_define_size 189
#ifdef __cplusplus
} /* extern "C" */

11
Core/Protobuf/PSource/bsp_PV.pb.h
View File

@ -14,6 +14,7 @@ typedef struct _PV_struct_define {
int32_t Robot_ChgLength;
double Robot_AutoSpeedBase;
double Robot_ManualSpeedBase;
int32_t Robot_LaneChange_Direction;
} PV_struct_define;
@ -22,19 +23,21 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define PV_struct_define_init_default {0, 0, 0}
#define PV_struct_define_init_zero {0, 0, 0}
#define PV_struct_define_init_default {0, 0, 0, 0}
#define PV_struct_define_init_zero {0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */
#define PV_struct_define_Robot_ChgLength_tag 1
#define PV_struct_define_Robot_AutoSpeedBase_tag 2
#define PV_struct_define_Robot_ManualSpeedBase_tag 3
#define PV_struct_define_Robot_LaneChange_Direction_tag 4
/* Struct field encoding specification for nanopb */
#define PV_struct_define_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, INT32, Robot_ChgLength, 1) \
X(a, STATIC, SINGULAR, DOUBLE, Robot_AutoSpeedBase, 2) \
X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 3)
X(a, STATIC, SINGULAR, DOUBLE, Robot_ManualSpeedBase, 3) \
X(a, STATIC, SINGULAR, INT32, Robot_LaneChange_Direction, 4)
#define PV_struct_define_CALLBACK NULL
#define PV_struct_define_DEFAULT NULL
@ -45,7 +48,7 @@ extern const pb_msgdesc_t PV_struct_define_msg;
/* Maximum encoded size of messages (where known) */
#define BSP_PV_PB_H_MAX_SIZE PV_struct_define_size
#define PV_struct_define_size 29
#define PV_struct_define_size 40
#ifdef __cplusplus
} /* extern "C" */

2
Core/Protobuf/PSource/bsp_ReCmd.pb.c
View File

@ -6,7 +6,7 @@
#error Regenerate this file with the current version of nanopb generator.
#endif
PB_BIND(ReCmd, ReCmd, 2)
PB_BIND(ReCmd, ReCmd, AUTO)

13
Core/Protobuf/PSource/bsp_ReCmd.pb.h
View File

@ -29,7 +29,7 @@ Buff_Data_Length 是下发的字节数， */
int32_t Parameter3;
int32_t Parameter4;
int32_t Buff_Data_Length;
pb_byte_t Buff_Data[500];
pb_callback_t Buff_Data;
} ReCmd;
@ -38,8 +38,8 @@ extern "C" {
#endif
/* Initializer values for message structs */
#define ReCmd_init_default {0, 0, 0, 0, 0, 0, 0, {0}}
#define ReCmd_init_zero {0, 0, 0, 0, 0, 0, 0, {0}}
#define ReCmd_init_default {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}}
#define ReCmd_init_zero {0, 0, 0, 0, 0, 0, 0, {{NULL}, NULL}}
/* Field tags (for use in manual encoding/decoding) */
#define ReCmd_CommadNum_tag 1
@ -60,8 +60,8 @@ X(a, STATIC, SINGULAR, INT32, Parameter2, 4) \
X(a, STATIC, SINGULAR, INT32, Parameter3, 5) \
X(a, STATIC, SINGULAR, INT32, Parameter4, 6) \
X(a, STATIC, SINGULAR, INT32, Buff_Data_Length, 7) \
X(a, STATIC, SINGULAR, FIXED_LENGTH_BYTES, Buff_Data, 8)
#define ReCmd_CALLBACK NULL
X(a, CALLBACK, SINGULAR, BYTES, Buff_Data, 8)
#define ReCmd_CALLBACK pb_default_field_callback
#define ReCmd_DEFAULT NULL
extern const pb_msgdesc_t ReCmd_msg;
@ -70,8 +70,7 @@ extern const pb_msgdesc_t ReCmd_msg;
#define ReCmd_fields &ReCmd_msg
/* Maximum encoded size of messages (where known) */
#define BSP_RECMD_PB_H_MAX_SIZE ReCmd_size
#define ReCmd_size 580
/* ReCmd_size depends on runtime parameters */
#ifdef __cplusplus
} /* extern "C" */

1
Core/Protobuf/Proto/bsp_GV.proto
View File

@ -31,7 +31,6 @@ message GV_struct_define
int32 Robot_ChgLength =18;
int32 Robot_ForceValue = 19;
int32 Robot_DynamometerValue = 20;
int32 Emergency = 21;
};

5
Core/Protobuf/Proto/bsp_IV.proto
View File

@ -13,12 +13,9 @@ message IV_struct_define{
int32 Robot_CurrentState= 8;
int32 Robot_Current_Left= 9;
int32 Robot_Current_Right= 10;
int32 Robot_Error_Left= 11;
int32 Robot_Error_Right= 12;
int32 Robot_Compensation_Left= 13;
int32 Robot_Compensation_Right= 14;
int32 Robot_RESET = 15;
};

3
Core/Protobuf/Proto/bsp_PV.proto
View File

@ -2,8 +2,7 @@ syntax = "proto3";
message PV_struct_define{
int32 Robot_ChgLength= 1;
double Robot_AutoSpeedBase=2;
double Robot_ManualSpeedBase=3;
int32 Robot_LaneChange_Direction = 4;
};

18
Core/Src/FSM.c
View File

@ -1,16 +1,15 @@
/*
* fsm.c
*
*状态机：通过遥控器的指令，判断机器人的运动状态。
* Created on: Oct 18, 2024
* Author: akeguo
*/
#include "fsm.h"
#include "BHBF_ROBOT.h"
#include "bsp_include.h"
#include "robot_state.h"
int32_t *RobotSpeed;
//int32_t JontSwingSpeed;
//int32_t JontTiltSpeed;
@ -265,13 +264,15 @@ void Front_End()
}
}
//LU_Three控制打磨推杆升降
//LU_Three控制打磨推杆升降:下压时，设置安全阈值，确保按压到一定值时，不再继续下压。
// GV.Robot_ForceValue即APP显示的压力值，设置10，是考虑到按压到一定程度后，开启打磨盘的压力余量
if(P_U7->LU_Three==1000)
{
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
}
else if(P_U7->LU_Three==-1000)
else if(P_U7->LU_Three==-1000 && GV.Robot_ForceValue < 10)
{
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
@ -328,6 +329,7 @@ void Emergency()
{
GV.Emergency = 1;
Operation_lock = 1; // 急停时自动锁定
Reset_Flag = 0;
}
// 2急停锁定计时（仅在急停状态下计数，解决松手不同时问题）
@ -362,6 +364,7 @@ void Emergency()
if (P_U7->LU_Three == 0)
{
Operation_lock = 0; // 满足条件，解锁
Reset_Flag = 0;
}
GF_BSP_GPIO_SetIO(0,0);
}
@ -386,6 +389,11 @@ void Emergency()
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
// 如果处于复位模式，直接停车，不再执行运动逻辑
if (system_mode == SYSTEM_RESET) {
HALT_State_Do();
return;
}
// //运动部分 Move Region
//左补偿、右补偿
Lcompensation_control();

3
Core/Src/main.c
View File

@ -253,12 +253,10 @@ void CV_GV_Init()
CV.TiltMoveSpeedBase = 1000;
Motor[1] = &GV.LeftFrontMotor; //Motor[1]指向GV的电机参�???????????
Motor[2] = &GV.RightFrontMotor;
RobotSpeed=&GV.Move_Speed;//RobotSpeed指向GV的移动车体�?�度 这是整体车�?? 不是单个轮的
AuTo_Flag=&GV.AuTo_Flag;
@ -280,7 +278,6 @@ void CV_GV_Init()
Motor_ID_Errors[1] = &GV.SystemErrorData.Motor_1_Error;
Motor_ID_Errors[2] = &GV.SystemErrorData.Motor_2_Error;
}
void GF_Robot_Init()

208
Core/Src/robot_state.c
View File

@ -1,10 +1,11 @@
/*
* robot_state.c
* robot_state.c 手动及自动控制 换道 PID
*
* Created on: Aug 2, 2024
* Author: akeguo
*/
#include "robot_state.h"
#include "FSM.h"
#include "bsp_GPIO.h"
#include <stdio.h>
#include <math.h>
@ -12,7 +13,7 @@
int Swing_Count=0;
int Home_Flag=0;
SystemMode_t system_mode = SYSTEM_NORMAL; // 默认开机是正常模式
ChgRoad_Parameters Chg_Pa =
{
@ -166,8 +167,9 @@ void HALT_State_Do(void)
Chg_Pa.change_road_status_flag = 0;
GV.Chg_Flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
// Polish_Flag = 0;
// Spray_Flag = 0;
}
@ -302,7 +304,6 @@ void VehicleAutoForward(double InclinometerAngle, double offsetAngle_left, dou
GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
// if (deltaAngle >= 0 && deltaAngle <= AM_Pa.deltaAngle1)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP1 * deltaSpeed;
@ -337,7 +338,6 @@ void VehicleAutoBackward(double InclinometerAngle, double offsetAngle_left, dou
}
GV.LeftFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed;
GV.RightFrontMotor.Target_Velcity = GV.Robot_AutoSpeed;
//
GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
@ -383,140 +383,187 @@ void VehicleAutoTurn(double InclinometerAngle, double expectationAngle, double K
}
void ChgUp()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
// 上换道
double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if (Chg_Pa.change_road_finish_flag == 0)
{
switch(Chg_Pa.change_road_status_flag)
switch (Chg_Pa.change_road_status_flag)
{
case 0:
if(angle >= -45
&& (angle <= 45))
case 0: // 初始化换道
if (angle >= -45 && angle <= 45)
{
Chg_Pa.change_road_status_flag=1;
Chg_Pa.change_road_status_flag = 1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10;
Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength - 10; // 使用 PV 配置的换道距离
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(90, (angle));
case 1: // 上转
VehicleAutoTurn(angle, 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转到90°前
continueTurn_up(90, angle);
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
Chg_Pa.change_road_status_flag = 2;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
Chg_Pa.isTurn = false; // 转完置位
//记录电机换道位置
// 记录电机换道位置，根据左右换道标志调整
if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道
{
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道
{
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
}
break;
case 2: //到了90°之后就开始水平后退
GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
case 2: // 水平后退
if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道
{
GV.LeftFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed);
}
else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道
{
GV.LeftFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed);
}
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m > 0 && n < 0)
if ((GV.PV.Robot_LaneChange_Direction == 0 && m > 0 && n < 0) ||
(GV.PV.Robot_LaneChange_Direction == 1 && m < 0 && n > 0))
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_status_flag = 3;
Chg_Pa.change_road_straight_count = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
}
break;
case 3:
VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(0, (angle));
case 3: // 转回来
VehicleAutoTurn(angle, 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转回0°前
continueTurn_up(0, angle);
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
Chg_Pa.change_road_status_flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
Chg_Pa.change_road_finish_flag = 1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
Chg_Pa.isTurn = false;
}
break;
default:
break;
}
}
}
void ChgDown()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
// 下换道
double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if (Chg_Pa.change_road_finish_flag == 0)
{
switch(Chg_Pa.change_road_status_flag)
switch (Chg_Pa.change_road_status_flag)
{
case 0:
if(angle >= -45
&& (angle <= 45))
case 0: // 初始化换道
if (angle >= -45 && angle <= 45)
{
Chg_Pa.change_road_status_flag=1;
Chg_Pa.change_road_status_flag = 1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength-10;
Chg_Pa.ChgWidth = GV.PV.Robot_ChgLength - 10; // 使用 PV 配置的换道距离
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(-90, (angle));
case 1: // 下转
VehicleAutoTurn(angle, -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转到-90°前
continueTurn_up(-90, angle);
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
Chg_Pa.change_road_status_flag = 2;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
Chg_Pa.isTurn = false; // 转完置位
//记录电机换道位置
// 记录电机换道位置，根据左右换道标志调整
if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道
{
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道
{
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Ini_Postition_R = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
}
break;
case 2: //到了-90°之后就开始水平前进
GV.LeftFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
case 2: // 水平后退
if (GV.PV.Robot_LaneChange_Direction == 0) // 左换道
{
GV.LeftFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed);
}
else if (GV.PV.Robot_LaneChange_Direction == 1) // 右换道
{
GV.LeftFrontMotor.Target_Velcity = WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity = -WheelMotorSpeedGet(Chg_Pa.speed);
}
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m < 0 && n > 0)
if ((GV.PV.Robot_LaneChange_Direction == 0 && m > 0 && n < 0) ||
(GV.PV.Robot_LaneChange_Direction == 1 && m < 0 && n > 0))
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_status_flag = 3;
Chg_Pa.change_road_straight_count = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
}
break;
case 3:
VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(0, (angle));
case 3: // 转回来
VehicleAutoTurn(angle, 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); // 转回0°前
continueTurn_up(0, angle);
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
Chg_Pa.change_road_status_flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
Chg_Pa.change_road_finish_flag = 1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
Chg_Pa.isTurn = false;
}
break;
default:
break;
}
@ -595,6 +642,7 @@ void ChgLeft()
}
void ChgRight()
{
//上换道
@ -665,10 +713,10 @@ void ChgRight()
}
}
void ChgFinish()
{
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
}

Write Preview
Loading…
Cancel
Save