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

305.竖直纠偏前进有补偿，后退没补偿

master
LIN\54376 2 weeks ago
parent
27f6b7dfea
commit
df71c739b4
10 changed files with 51 additions and 58 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .settings/language.settings.xml
  2. 19
      BASE/Src/MSP/msp_U7.c
  3. 3
      Core/Inc/FSM.h
  4. 3
      Core/Inc/robot_state.h
  5. 14
      Core/Protobuf/PSource/bsp_IV.pb.h
  6. 20
      Core/Protobuf/Proto/bsp_CV.proto
  7. 8
      Core/Protobuf/Proto/bsp_PV.proto
  8. 13
      Core/Src/FSM.c
  9. 9
      Core/Src/main.c
  10. 18
      Core/Src/robot_state.c

4
.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <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.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" 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="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"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1058933426992106082" 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.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/> <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.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" 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="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"> <provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1058933426992106082" 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.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/> <language-scope id="org.eclipse.cdt.core.g++"/>
</provider> </provider>

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

@ -35,7 +35,7 @@ void decode_U7Data(uint8_t *buffer, uint16_t length)
{ {
system_mode = SYSTEM_RESET; system_mode = SYSTEM_RESET;
CurrentMoveState = Move_Reset; CurrentMoveState = Move_Reset;
HALT_State_Do(); // 立即停车并清空状态
} }
else else
{ {
@ -56,7 +56,7 @@ void decode_U7Data(uint8_t *buffer, uint16_t length)
else else
{ {
// 还没复位，保持 RESET // 还没复位，保持 RESET
HALT_State_Do(); CurrentMoveState = Move_Reset;
} }
} }
} }
@ -132,16 +132,17 @@ uint8_t IsRemoteAtInitialPosition(void)
{ {
if(GV.U7_Key.CH0_LYB_V != 0) return 0; if(GV.U7_Key.CH0_LYB_V != 0) return 0;
if(GV.U7_Key.CH1_LYB_H != 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.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.M5 != -1000) return 0;
if(GV.U7_Key.M6 != -1000) return 0; if(GV.U7_Key.M3 != -1000) return 0;
if(GV.U7_Key.RU_Single != -1000) return 0; if(GV.U7_Key.LU_pulley != 0) return 0;
if(GV.U7_Key.RU_pulley != 0) return 0;
if(GV.U7_Key.LU_Three != 0) return 0;
if(GV.U7_Key.RU_Three != 0) return 0; if(GV.U7_Key.RU_Three != 0) return 0;
if(GV.U7_Key.LU_Single != -1000) return 0;
if(GV.U7_Key.RU_Single != -1000) return 0;
if(GV.U7_Key.M2 != -1000) return 0;
if(GV.U7_Key.M6 != -1000) return 0;
LOG("U7_At_Initial_Posotion"); LOG("U7_At_Initial_Posotion");
return 1; // 全部归零 return 1; // 全部归零
} }

3
Core/Inc/FSM.h
View File

@ -44,15 +44,12 @@ typedef enum _MoveSTATE_t
Move_Backwards, Move_Backwards,
Move_TurnLeft, Move_TurnLeft,
Move_TurnRight, Move_TurnRight,
Move_AutoForward, Move_AutoForward,
Move_AutoBackward, Move_AutoBackward,
Move_ChgLeft, Move_ChgLeft,
Move_ChgRight, Move_ChgRight,
Move_ChgUp, Move_ChgUp,
Move_ChgDown, Move_ChgDown,
Move_ChgFinish, Move_ChgFinish,
Move_Emergency, Move_Emergency,
Move_Reset, Move_Reset,

3
Core/Inc/robot_state.h
View File

@ -78,9 +78,8 @@ void Forwards_State_Do(void);
void Backwards_State_Do(void); void Backwards_State_Do(void);
void TurnLeft_State_Do(void); void TurnLeft_State_Do(void);
void TurnRight_State_Do(void); void TurnRight_State_Do(void);
void HALT_State_Do(void);//停止 void HALT_State_Do(void);//停止
void Reset(void);
void SwingHALT_State_Do(void); void SwingHALT_State_Do(void);
void SwingLeft_State_Do(void); void SwingLeft_State_Do(void);

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

@ -27,8 +27,6 @@ typedef struct _IV_struct_define {
int32_t Robot_Compensation_Left; int32_t Robot_Compensation_Left;
int32_t Robot_Compensation_Right; int32_t Robot_Compensation_Right;
int32_t Robot_RESET; int32_t Robot_RESET;
int32_t Robot_LaneChange_Left;
int32_t Robot_LaneChange_Right;
} IV_struct_define; } IV_struct_define;
@ -37,8 +35,8 @@ extern "C" {
#endif #endif
/* Initializer values for message structs */ /* 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, 0, 0, 0} #define IV_struct_define_init_default {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} #define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
/* Field tags (for use in manual encoding/decoding) */ /* Field tags (for use in manual encoding/decoding) */
#define IV_struct_define_Robot_Move_AutoSpeed_tag 1 #define IV_struct_define_Robot_Move_AutoSpeed_tag 1
@ -56,8 +54,6 @@ extern "C" {
#define IV_struct_define_Robot_Compensation_Left_tag 13 #define IV_struct_define_Robot_Compensation_Left_tag 13
#define IV_struct_define_Robot_Compensation_Right_tag 14 #define IV_struct_define_Robot_Compensation_Right_tag 14
#define IV_struct_define_Robot_RESET_tag 15 #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 */ /* Struct field encoding specification for nanopb */
#define IV_struct_define_FIELDLIST(X, a) \ #define IV_struct_define_FIELDLIST(X, a) \
@ -75,9 +71,7 @@ X(a, STATIC, SINGULAR, INT32, Robot_Error_Left, 11) \
X(a, STATIC, SINGULAR, INT32, Robot_Error_Right, 12) \ 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_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_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_CALLBACK NULL
#define IV_struct_define_DEFAULT NULL #define IV_struct_define_DEFAULT NULL
@ -88,7 +82,7 @@ extern const pb_msgdesc_t IV_struct_define_msg;
/* Maximum encoded size of messages (where known) */ /* Maximum encoded size of messages (where known) */
#define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size #define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size
#define IV_struct_define_size 189 #define IV_struct_define_size 165
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

20
Core/Protobuf/Proto/bsp_CV.proto
View File

@ -4,18 +4,14 @@ syntax = "proto3";
message CV_struct_define{ message CV_struct_define{
//洗舱项目 //洗舱项目
int32 RobotMoveSpeedBase = 2; int32 RobotMoveSpeedBase = 2;
int32 SwingMoveSpeedBase =3; int32 SwingMoveSpeedBase =3;
int32 TiltMoveSpeedBase = 4; int32 TiltMoveSpeedBase = 4;
int32 TiltMoveTargetSpeed = 5; int32 TiltMoveTargetSpeed = 5;
int32 SwingMoveTargetSpeed = 6; int32 SwingMoveTargetSpeed = 6;
int32 TurnLeftRightSpeed = 7; int32 TurnLeftRightSpeed = 7;
}; };

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

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

13
Core/Src/FSM.c
View File

@ -53,7 +53,7 @@ transition_t MoveTransitions[] =
{ Move_ChgUp, ChgUp}, { Move_ChgUp, ChgUp},
{ Move_ChgDown, ChgDown}, { Move_ChgDown, ChgDown},
{ Move_ChgFinish, ChgFinish}, { Move_ChgFinish, ChgFinish},
{ Move_Reset, Reset},
}; };
transition_t SwingTransitions[] = transition_t SwingTransitions[] =
@ -264,15 +264,13 @@ void Front_End()
} }
} }
//LU_Three控制打磨推杆升降:下压时，设置安全阈值，确保按压到一定值时，不再继续下压。 //LU_Three控制打磨推杆升降
// GV.Robot_ForceValue即APP显示的压力值，设置10，是考虑到按压到一定程度后，开启打磨盘的压力余量
if(P_U7->LU_Three==1000) if(P_U7->LU_Three==1000)
{ {
GF_BSP_GPIO_SetIO(Out_Lift_2,0); GF_BSP_GPIO_SetIO(Out_Lift_2,0);
GF_BSP_GPIO_SetIO(Out_Lift_1,1); GF_BSP_GPIO_SetIO(Out_Lift_1,1);
} }
else if(P_U7->LU_Three==-1000 && GV.Robot_ForceValue < 10) else if(P_U7->LU_Three==-1000 && GV.Robot_ForceValue < 100)
{ {
GF_BSP_GPIO_SetIO(Out_Lift_2,1); GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,0); GF_BSP_GPIO_SetIO(Out_Lift_1,0);
@ -391,10 +389,10 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{ {
// 如果处于复位模式，直接停车，不再执行运动逻辑 // 如果处于复位模式，直接停车，不再执行运动逻辑
if (system_mode == SYSTEM_RESET) { if (system_mode == SYSTEM_RESET) {
HALT_State_Do(); action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
return; return;
} }
// //运动部分 Move Region //运动部分 Move Region
//左补偿、右补偿 //左补偿、右补偿
Lcompensation_control(); Lcompensation_control();
Rcompensation_control(); Rcompensation_control();
@ -407,7 +405,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的 CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9; GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9;
//
IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll; IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll;
IV.Robot_ForceValue = GV.Robot_ForceValue; IV.Robot_ForceValue = GV.Robot_ForceValue;
IV.Robot_DynamometerValue = GV.Robot_DynamometerValue; IV.Robot_DynamometerValue = GV.Robot_DynamometerValue;

9
Core/Src/main.c
View File

@ -32,7 +32,6 @@
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "BHBF_ROBOT.h" #include "BHBF_ROBOT.h"
#include "fsm.h" #include "fsm.h"
#include "motors.h" #include "motors.h"
#include "robot_state.h" #include "robot_state.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
@ -103,9 +102,9 @@ int main(void)
/* USER CODE END 1 */ /* USER CODE END 1 */
/* MPU Configuration--------------------------------------------------------*/ /* MPU Configuration--------------------------------------------------------*/
MPU_Config(); MPU_Config();
/* Enable I-Cache---------------------------------------------------------*/ /* Enable I-Cache------------------- --------------------------------------*/
SCB_EnableICache(); SCB_EnableICache();
/* Enable D-Cache---------------------------------------------------------*/ /* Enable D-Cache---------------------------------------------------------*/
@ -151,7 +150,7 @@ int main(void)
GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�??????????? GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�???????????
GF_BSP_GPIO_SetIO(1,1); GF_BSP_GPIO_SetIO(1,1);
HAL_Delay(10000); ////////////////////////////延时10s HAL_Delay(5000); ////////////////////////////延时5s
DLT_LOG_ENABLE_LEVEL=0;//7 send all information 0 send nothing DLT_LOG_ENABLE_LEVEL=0;//7 send all information 0 send nothing
Error_Detect_Intialzie(500);//every 1 seconds Error_Detect_Intialzie(500);//every 1 seconds
@ -264,7 +263,7 @@ void CV_GV_Init()
Motor[2]->MotorID = 2; Motor[2]->MotorID = 2;
P_U7 = &GV.U7_Key;//P_MK32 指向GV的遥控器参数 P_U7 = &GV.U7_Key;//P_U7 指向GV的遥控器参数
SP_MSP_RF_TL720D_Parameters_In = &GV.Robot_Angle ;// SP_MSP_RF_TL720D_Parameters_In 指向GV的�?�角 SP_MSP_RF_TL720D_Parameters_In = &GV.Robot_Angle ;// SP_MSP_RF_TL720D_Parameters_In 指向GV的�?�角
CMCUValue_in = &GV.Robot_ForceValue; CMCUValue_in = &GV.Robot_ForceValue;

18
Core/Src/robot_state.c
View File

@ -168,10 +168,20 @@ void HALT_State_Do(void)
GV.Chg_Flag = 0; GV.Chg_Flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0; GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0; GV.RightFrontMotor.Target_Velcity = 0;
// Polish_Flag = 0; // GF_BSP_GPIO_SetIO(Out_Polish, 1);
// Spray_Flag = 0; // GF_BSP_GPIO_SetIO(Out_Spray, 1);
}
void Reset(void)
{
*AuTo_Flag = 0;
Chg_Pa.change_road_finish_flag = 0;
Chg_Pa.change_road_status_flag = 0;
GV.Chg_Flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
GF_BSP_GPIO_SetIO(Out_Polish, 1);
GF_BSP_GPIO_SetIO(Out_Spray, 1);
} }
void continueTurn_up(double Angle, double CurrentAngle) void continueTurn_up(double Angle, double CurrentAngle)
{ {
@ -276,7 +286,7 @@ void Auto()
//垂直 //垂直
if ((AM_Pa.ExpectationAngle > -45 && AM_Pa.ExpectationAngle < 45)) if ((AM_Pa.ExpectationAngle > -45 && AM_Pa.ExpectationAngle < 45))
{ {
AM_Pa.ExpectationAngle = 0 - GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0; AM_Pa.ExpectationAngle = 0 ; //- GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0;
} }
VehicleAutoBackward(angle, VehicleAutoBackward(angle,
GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动后退 GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动后退

Write Preview
Loading…
Cancel
Save