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

新增左右滑轮调节左右补偿

master
LIN\54376 1 month ago
parent
0bd734d0f5
commit
59c441387f
10 changed files with 99 additions and 65 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. 24
      BASE/Inc/BSP/BHBF_ROBOT.h
  2. 39
      BASE/Src/BSP/BHBF_ROBOT.c
  3. 2
      Core/Protobuf/PSource/bsp_GV.pb.h
  4. 14
      Core/Protobuf/PSource/bsp_IV.pb.h
  5. 20
      Core/Protobuf/PSource/bsp_PV.pb.h
  6. 3
      Core/Protobuf/Proto/bsp_IV.proto
  7. 6
      Core/Protobuf/Proto/bsp_PV.proto
  8. 38
      Core/Src/FSM.c
  9. 2
      Core/Src/main.c
  10. 16
      Core/Src/robot_state.c

24
BASE/Inc/BSP/BHBF_ROBOT.h
View File

@ -11,7 +11,7 @@
#include "left_right_compensation_control.h"
#include "../../BASE/Inc/BSP/bsp_include.h" #include "../../BASE/Inc/BSP/bsp_include.h"
#include "bsp_PV.pb.h" #include "bsp_PV.pb.h"
@ -67,6 +67,28 @@
#include "../../BASE/Inc/BSP/bsp_Error_Detect.h" #include "../../BASE/Inc/BSP/bsp_Error_Detect.h"
#include "bsp_IV.pb.h" #include "bsp_IV.pb.h"
#include "bsp_PV.pb.h" #include "bsp_PV.pb.h"
typedef struct sys_timer_handler
{
int start_timer;
int sys_current_timer_count;
int sys_timer_flag;
}Sys_timer_handler;
extern Sys_timer_handler timer_handler_1;
extern Sys_timer_handler timer_handler_2;
extern Sys_timer_handler timer_handler_3;
extern Sys_timer_handler timer_handler_4;
//first start Timer, then WaitTimer
extern bool CompareTimer(int32_t DelayMiliSeconds,Sys_timer_handler * timer_handler);
extern void SystemTimer_Intialize();
extern void GF_Timer_Count();
//#include "msp_pressure_sensor.h" //#include "msp_pressure_sensor.h"
//#include "robot_state.h" //#include "robot_state.h"
//FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 896K //FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 896K

39
BASE/Src/BSP/BHBF_ROBOT.c
View File

@ -7,12 +7,15 @@
#include "fsm.h" #include "fsm.h"
#include "BHBF_ROBOT.h" #include "BHBF_ROBOT.h"
#include <string.h> #include <string.h>
#include "bsp_FDCAN.h" #include "bsp_FDCAN.h"
int32_t SystemTimeMiliCount; //2ms加一
//初始化定时器
Sys_timer_handler timer_handler_1;
Sys_timer_handler timer_handler_2;
Sys_timer_handler timer_handler_3;
Sys_timer_handler timer_handler_4;
//7 motors //7 motors
MotorParameters *Motor[3]; MotorParameters *Motor[3];
@ -65,3 +68,33 @@ void GF_WatchDog_Loop()
} }
//比较计数值
bool CompareTimer(int32_t DelayMiliSeconds,Sys_timer_handler * timer_handler)
{
if(timer_handler->start_timer==1)
{
timer_handler->sys_current_timer_count=DelayMiliSeconds/2+SystemTimeMiliCount;
timer_handler->start_timer=0;
}
if(timer_handler->sys_current_timer_count<=SystemTimeMiliCount)
{
return true;
}else
{
return false;
}
}
//定时数数
void GF_Timer_Count()
{
SystemTimeMiliCount++;
}
//将数数写入定时器
void SystemTimer_Intialize()
{
SystemTimeMiliCount=0;
GF_BSP_Interrupt_Add_CallBack(
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Timer_Count);
}

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) */ /* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size
#define GV_struct_define_size 902 #define GV_struct_define_size 884
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

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

@ -24,6 +24,8 @@ typedef struct _IV_struct_define {
int32_t Robot_Current_Right; int32_t Robot_Current_Right;
int32_t Robot_Error_Left; int32_t Robot_Error_Left;
int32_t Robot_Error_Right; int32_t Robot_Error_Right;
int32_t Robot_Compensation_Left;
int32_t Robot_Compensation_Right;
} IV_struct_define; } IV_struct_define;
@ -32,8 +34,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} #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} #define IV_struct_define_init_zero {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
@ -48,6 +50,8 @@ extern "C" {
#define IV_struct_define_Robot_Current_Right_tag 10 #define IV_struct_define_Robot_Current_Right_tag 10
#define IV_struct_define_Robot_Error_Left_tag 11 #define IV_struct_define_Robot_Error_Left_tag 11
#define IV_struct_define_Robot_Error_Right_tag 12 #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
/* 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) \
@ -62,7 +66,9 @@ X(a, STATIC, SINGULAR, INT32, Robot_CurrentState, 8) \
X(a, STATIC, SINGULAR, INT32, Robot_Current_Left, 9) \ X(a, STATIC, SINGULAR, INT32, Robot_Current_Left, 9) \
X(a, STATIC, SINGULAR, INT32, Robot_Current_Right, 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_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_Right, 14)
#define IV_struct_define_CALLBACK NULL #define IV_struct_define_CALLBACK NULL
#define IV_struct_define_DEFAULT NULL #define IV_struct_define_DEFAULT NULL
@ -73,7 +79,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 132 #define IV_struct_define_size 154
#ifdef __cplusplus #ifdef __cplusplus
} /* extern "C" */ } /* extern "C" */

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

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

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

@ -17,4 +17,7 @@ message IV_struct_define{
int32 Robot_Error_Left= 11; int32 Robot_Error_Left= 11;
int32 Robot_Error_Right= 12; int32 Robot_Error_Right= 12;
int32 Robot_Compensation_Left= 13;
int32 Robot_Compensation_Right= 14;
}; };

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

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

38
Core/Src/FSM.c
View File

@ -100,6 +100,8 @@ int JJ_Flag;
void GF_Dispatch()//2ms调用一次 给车体速度等赋值 void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{ {
// //运动部分 Move Region // //运动部分 Move Region
Lcompensation_control();
Rcompensation_control();
int b=Get_BIT(SystemErrorCode, u7_sbus) ; int b=Get_BIT(SystemErrorCode, u7_sbus) ;
if (Get_BIT(SystemErrorCode, u7_sbus) == 1) if (Get_BIT(SystemErrorCode, u7_sbus) == 1)
@ -310,16 +312,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
} }
//控制电磁阀喷枪：旧
// if(P_U7->RU_Single==1000) // && CurrentMoveState != Move_AutoForward&& CurrentMoveState != Move_AutoBackward)
// {
// GF_BSP_GPIO_SetIO(Out_Spray,0);
// }
// else if(P_U7->RU_Single==-1000) // && CurrentMoveState != Move_AutoForward && CurrentMoveState != Move_AutoBackward)
// {
// GF_BSP_GPIO_SetIO(Out_Spray,1);
// }
// 控制电磁阀喷枪（非自锁版本） // 控制电磁阀喷枪（非自锁版本）
// 功能：按一下开启，再按一下关闭，长按不会反复切换 // 功能：按一下开启，再按一下关闭，长按不会反复切换
@ -441,39 +433,23 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9; GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9;
// //
// IV.Robot_Move_AutoSpeed = GV.PV.Robot_AutoSpeedBase;//好像是往安卓界面传数据的
// IV.Robot_Move_ManualSpeed = GV.PV.Robot_ManualSpeedBase;//好像是往安卓界面传数据的
// IV.Robot_Swing_Speed=GV.SwingMotor.Velcity;
// IV.Robot_Tilt_Speed=GV.TiltMotor.Velcity;
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;
IV.Robot_Current_Left = GV.LeftFrontMotor.Current; IV.Robot_Current_Left = GV.LeftFrontMotor.Current;
IV.Robot_Current_Right = GV.RightFrontMotor.Current; IV.Robot_Current_Right = GV.RightFrontMotor.Current;
IV.Robot_Error = 0;
if(GV.LeftFrontMotor.ERROR_Flag != 0) IV.Robot_Compensation_Left = GV.Left_Compensation;
{ IV.Robot_Compensation_Right = GV.Right_Compensation;
IV.Robot_Error = 100+GV.LeftFrontMotor.ERROR_Flag;
}
else if(GV.RightFrontMotor.ERROR_Flag != 0)
{
IV.Robot_Error = 200+GV.RightFrontMotor.ERROR_Flag;
}
else if(GV.LeftFrontMotor.ERROR_Flag != 0 && GV.RightFrontMotor.ERROR_Flag != 0)
{
IV.Robot_Error = 10000 + GV.LeftFrontMotor.ERROR_Flag * 100 + GV.RightFrontMotor.ERROR_Flag;
}
IV.Robot_Error_Left = GV.LeftFrontMotor.ERROR_Flag; IV.Robot_Error_Left = GV.LeftFrontMotor.ERROR_Flag;
IV.Robot_Error_Right = GV.RightFrontMotor.ERROR_Flag; IV.Robot_Error_Right = GV.RightFrontMotor.ERROR_Flag;
IV.Robot_CurrentState = CurrentMoveState; IV.Robot_CurrentState = CurrentMoveState;
// GV.Robot_ChgLength = CV.PV.Robot_ChgLength - 10;
// GV.Left_Compensation = CV.PV.Robot_LeftCompensation;
// GV.Right_Compensation = CV.PV.Robot_RightCompensation;
action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态 action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
// action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState); // action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState);

2
Core/Src/main.c
View File

@ -147,6 +147,8 @@ int main(void)
MX_LPUART1_UART_Init(); MX_LPUART1_UART_Init();
MX_ADC2_Init(); MX_ADC2_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
SystemTimer_Intialize(); //加数数定时器
GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�??????????? GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�???????????
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

16
Core/Src/robot_state.c
View File

@ -191,17 +191,17 @@ void Auto()
//水平 //水平
if ((angle >= 45 && angle <= 135)) if ((angle >= 45 && angle <= 135))
{ {
AM_Pa.ExpectationAngle = 90 - GV.PV.Robot_RightCompensation; AM_Pa.ExpectationAngle = 90 - GV.Right_Compensation/100.0;
} }
else if ((angle >= -135 && angle <= -45)) else if ((angle >= -135 && angle <= -45))
{ {
AM_Pa.ExpectationAngle = -90 + GV.PV.Robot_LeftCompensation; AM_Pa.ExpectationAngle = -90 + GV.Left_Compensation/100.0;
} }
//垂直 //垂直
else if ((angle > -45 && angle < 45)) else if ((angle > -45 && angle < 45))
{ {
AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; AM_Pa.ExpectationAngle = 0 - GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0;
} }
VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
@ -216,24 +216,24 @@ void Auto()
break; break;
case 2: case 2:
VehicleAutoForward(angle, VehicleAutoForward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进 GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动前进
break; break;
case 3: case 3:
//水平 //水平
if ((angle >= 45 && angle <= 135)) if ((angle >= 45 && angle <= 135))
{ {
AM_Pa.ExpectationAngle = 90 + GV.PV.Robot_LeftCompensation; AM_Pa.ExpectationAngle = 90 + GV.Left_Compensation/100.0;
} }
if ((angle >= -135 && angle <= -45)) if ((angle >= -135 && angle <= -45))
{ {
AM_Pa.ExpectationAngle = - 90 - GV.PV.Robot_RightCompensation; AM_Pa.ExpectationAngle = - 90 - GV.Right_Compensation/100.0;
} }
//垂直 //垂直
if ((angle > -45 && angle < 45)) if ((angle > -45 && angle < 45))
{ {
AM_Pa.ExpectationAngle = 0 - GV.PV.Robot_LeftCompensation + GV.PV.Robot_RightCompensation; AM_Pa.ExpectationAngle = 0 - GV.Left_Compensation/100.0 + GV.Right_Compensation/100.0;
} }
VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转 VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
@ -248,7 +248,7 @@ void Auto()
break; break;
case 4: case 4:
VehicleAutoBackward(angle, VehicleAutoBackward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退 GV.Left_Compensation/100.0, GV.Right_Compensation/100.0);//自动后退
break; break;

Write Preview
Loading…
Cancel
Save