LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

纠偏走添加速度参数

paint_robot_new-v1.5
Lizongdi 2 weeks ago
parent
e8cf73a38f
commit
d005912874
4 changed files with 24 additions and 14 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. 3
      RBcore/TL720D.c
  2. 1
      RBcore/include/BHBF.h
  3. 8
      RBcore/msp_Timer.c
  4. 26
      project/paint_robot_new/paint_robot_new.c

3
RBcore/TL720D.c
View File

@ -41,7 +41,7 @@
* 模块级变量 * * 模块级变量 *
*----------------------------------------------*/ *----------------------------------------------*/
static MSP_TL720DParameters g_stTL720D = {0}; static MSP_TL720DParameters g_stTL720D = {0};
volatile int32_t g_RF_Angle_Roll = 0; volatile int32_t g_RF_Angle_Roll = 0; // 注意这个全局是为了解决PID实时性问题添加的，不能在其他位置使用
/*----------------------------------------------* /*----------------------------------------------*
* 常量定义 * * 常量定义 *
@ -122,6 +122,7 @@ static void decode_TL720D(const char *buf, uint32_t _iSize)
g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]); g_stTL720D.RF_Gro_Z = getDeci((uint8_t *)&buf[28]);
g_RF_Angle_Roll = g_stTL720D.RF_Angle_Roll; g_RF_Angle_Roll = g_stTL720D.RF_Angle_Roll;
// 注意g_RF_Angle_Roll这个全局是为了解决PID实时性问题添加的，不能在其他位置使用
uint32_t uiNowTick = Rd_GetTime(); uint32_t uiNowTick = Rd_GetTime();
if ((int32_t)(uiNowTick - uiLastSendTick) >= 50) if ((int32_t)(uiNowTick - uiLastSendTick) >= 50)
{ {

1
RBcore/include/BHBF.h
View File

@ -125,6 +125,7 @@ typedef struct {
int m_iMode; //正数表示前进，负数表示倒退 int m_iMode; //正数表示前进，负数表示倒退
int m_iAngle; //目标角度 int m_iAngle; //目标角度
int m_iTime; //行驶时间(单位毫秒)计时，负数表示一直行走 int m_iTime; //行驶时间(单位毫秒)计时，负数表示一直行走
int m_iSpeed; //车速
} BHBF_straight_drive_Cmd; } BHBF_straight_drive_Cmd;
/*==============================================* /*==============================================*
* project-wide global variables * * project-wide global variables *

8
RBcore/msp_Timer.c
View File

@ -102,13 +102,13 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg)
} }
if (stCmd.m_iMode > 0) if (stCmd.m_iMode > 0)
{ {
aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; aiMotorSpeed[0] = (stCmd.m_iSpeed * 10) - g_dletAngle ;
aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; aiMotorSpeed[1] = (stCmd.m_iSpeed * 10) + g_dletAngle ;
} }
else else
{ {
aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; aiMotorSpeed[0] = -(stCmd.m_iSpeed * 10) - g_dletAngle ;
aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; aiMotorSpeed[1] = -(stCmd.m_iSpeed * 10) + g_dletAngle ;
} }
} }
else else

26
project/paint_robot_new/paint_robot_new.c
View File

@ -119,6 +119,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
static int iLeft_Compensation = 0; static int iLeft_Compensation = 0;
static int iRight_Compensation = 0; static int iRight_Compensation = 0;
static int iVehicleSpeed = 1;
switch (pstMsg->m_uiMsgID) switch (pstMsg->m_uiMsgID)
{ {
case CUSTOM_GET_DAEMON_CODE: case CUSTOM_GET_DAEMON_CODE:
@ -198,7 +199,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -207,7 +209,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation, .m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -219,7 +222,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -228,7 +232,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation, .m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin() .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -307,7 +312,6 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
// 更新速度旋钮值 // 更新速度旋钮值
int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200; int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
int iVehicleSpeed = 1;
if (iSpeedSelection > 0) if (iSpeedSelection > 0)
{ {
iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30; iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
@ -405,7 +409,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1,
.m_iSpeed = iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -426,7 +431,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1,
.m_iSpeed = iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -460,7 +466,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = 1, .m_iMode = 1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1,
.m_iSpeed = iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }
@ -480,7 +487,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
BHBF_straight_drive_Cmd stCmd = { BHBF_straight_drive_Cmd stCmd = {
.m_iMode = -1, .m_iMode = -1,
.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration, .m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
.m_iTime = -1 .m_iTime = -1,
.m_iSpeed = iVehicleSpeed
}; };
MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
} }

Write Preview
Loading…
Cancel
Save