You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
321 lines
8.0 KiB
321 lines
8.0 KiB
3 weeks ago
|
/*
|
||
|
|
* robot_state.c
|
||
|
|
*
|
||
|
|
* Created on: Aug 2, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "robot_state.h"
|
||
|
|
#include "fsm.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
#include "BSP/bsp_GPIO.h"
|
||
|
|
#include "MSP/msp_PID.h"
|
||
|
|
#include "msp_DMKE_485_Motor.h"
|
||
|
|
|
||
|
|
int32_t *DHRoughening_Speed[10]; //目标速度
|
||
|
|
|
||
|
|
int32_t *DHRoughening_LaneChangeDistance[10]; //目标速度
|
||
|
|
|
||
|
|
int32_t *Roughenging_LeftCompensation;
|
||
|
|
int32_t *Roughenging_RightCompensation;
|
||
|
|
int32_t *Roughenging_LeftCompensation_Range;
|
||
|
|
int32_t *Roughenging_RightCompensation_Range;
|
||
|
|
|
||
|
|
//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
|
||
|
|
void Calbrate_Robot_Positon(int position);
|
||
|
|
int32_t Horizontal_Target_Angle = 0;
|
||
|
|
|
||
|
|
void Move_Forwards_Do(int32_t Target_Angle);
|
||
|
|
void Move_Backwards_Do(int32_t Target_Angle);
|
||
|
|
|
||
|
|
void Abnormal_State_Do(void)
|
||
|
|
{
|
||
|
|
LOG("Abnormal_State_Do");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Forwards_State_Do(void)
|
||
|
|
{
|
||
|
|
if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
{
|
||
|
|
IV.RunMode = IV_Run_Mode_Forward;
|
||
|
|
}
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Backwards_State_Do(void)
|
||
|
|
{
|
||
|
|
// if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
// {
|
||
|
|
// IV.RunMode = IV_Run_Mode_BackWard;
|
||
|
|
// }
|
||
|
|
IV.RunMode = IV_Run_Mode_BackWard;
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed;
|
||
|
|
}
|
||
|
|
void TurnLeft_State_Do(void)
|
||
|
|
{
|
||
|
|
|
||
|
|
// if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
// {
|
||
|
|
// IV.RunMode = IV_Run_Mode_TurnLeft;
|
||
|
|
// }
|
||
|
|
IV.RunMode = IV_Run_Mode_TurnLeft;
|
||
|
|
GV.LeftMotor.Target_Velcity = (int32_t) (-CV.LeftTurnSpeed / 0.880 * 101 * 10);
|
||
|
|
GV.RightMotor.Target_Velcity = (int32_t) (CV.LeftTurnSpeed / 0.880 * 101 * 10);
|
||
|
|
}
|
||
|
|
void TurnRight_State_Do(void)
|
||
|
|
{
|
||
|
|
// if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
// {
|
||
|
|
// IV.RunMode = IV_Run_Mode_TurnRight;
|
||
|
|
// }
|
||
|
|
IV.RunMode = IV_Run_Mode_TurnRight;
|
||
|
|
GV.LeftMotor.Target_Velcity = (int32_t) (CV.RightTurnSpeed / 0.880 * 101 * 10);
|
||
|
|
GV.RightMotor.Target_Velcity = (int32_t) (-CV.RightTurnSpeed / 0.880 * 101 * 10);
|
||
|
|
}
|
||
|
|
void HALT_State_Do(void)
|
||
|
|
{
|
||
|
|
|
||
|
|
// if (IV.RunMode != IV_Run_Mode_EmergencyStop && IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
// {
|
||
|
|
// IV.RunMode = IV_Run_Mode_HALT;
|
||
|
|
// }
|
||
|
|
IV.RunMode = IV_Run_Mode_HALT;
|
||
|
|
GV.LeftMotor.Target_Velcity = 0;
|
||
|
|
GV.RightMotor.Target_Velcity = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
//竖直方向前进 保持竖直运行
|
||
|
|
static int dletAngle = 0;
|
||
|
|
|
||
|
|
void Move_Vertical_Task_Forwards_Do()
|
||
|
|
{
|
||
|
|
Move_Forwards_Do(CV.RobotUpAngleValue + PV_App.Vertical_Calibration * 100);
|
||
|
|
}
|
||
|
|
//竖直方向后退前进 保持竖直运行
|
||
|
|
void Move_Vertical_Task_Backwards_Do()
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotUpAngleValue + PV_App.Vertical_Calibration * 100);
|
||
|
|
}
|
||
|
|
|
||
|
|
char is_within_the_target_angle_flag = 0;
|
||
|
|
//水平方向前进，保持水平方向
|
||
|
|
void Move_Forwards_Do(int32_t Target_Angle)
|
||
|
|
{
|
||
|
|
if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
{
|
||
|
|
IV.RunMode = IV_Run_Mode_Forward;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= 1000) //100倍关系，大于10度
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) < 100)
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 10, 0, 8, 1000);
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed + dletAngle / 2;
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 2 / 0.880 * 101 * 10 * 2);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
// GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle / 2;
|
||
|
|
// GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed + dletAngle / 2;
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 4 / 0.880 * 101 * 10 * 2);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
//水平方向后退，保持水平方向
|
||
|
|
|
||
|
|
void Move_Backwards_Do(int32_t Target_Angle)
|
||
|
|
{
|
||
|
|
if (IV.RunMode != IV_Run_Mode_Automation)
|
||
|
|
{
|
||
|
|
IV.RunMode = IV_Run_Mode_BackWard;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= 1000) //100倍关系，大于10度
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) < 100)
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 10, 0, 8, 1000);
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle / 2;
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 2 / 0.880 * 101 * 10 * 2);
|
||
|
|
// GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle / 2;
|
||
|
|
// GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle / 2;
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 4 / 0.880 * 101 * 10 * 2);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Move_Horizontal_Task_Forwards_Right_Do()
|
||
|
|
{
|
||
|
|
Move_Forwards_Do(CV.RobotRightAngleValue - GV.Left_Compensation);
|
||
|
|
|
||
|
|
//Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation);
|
||
|
|
}
|
||
|
|
void Move_Horizontal_Task_Backwards_Right_Do()
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);
|
||
|
|
|
||
|
|
//Move_Backwards_Do(CV.RobotRightAngleValue + GV.Right_Compensation);
|
||
|
|
}
|
||
|
|
void Move_Horizontal_Task_Forwards_Left_Do()
|
||
|
|
{
|
||
|
|
Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Right_Compensation);
|
||
|
|
|
||
|
|
//Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);
|
||
|
|
|
||
|
|
}
|
||
|
|
void Move_Horizontal_Task_Backwards_Left_Do()
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);
|
||
|
|
|
||
|
|
//Move_Forwards_Do(CV.RobotLeftAngleValue - GV.Left_Compensation);
|
||
|
|
}
|
||
|
|
|
||
|
|
void Move_Head_To_Left_Do(void)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotLeftAngleValue);
|
||
|
|
//Calbrate_Robot_Positon(CV.RobotLeftAngleValue - GV.Right_Compensation);
|
||
|
|
}
|
||
|
|
void Move_Head_To_Right_Do(void)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotRightAngleValue );
|
||
|
|
//Calbrate_Robot_Positon(CV.RobotRightAngleValue + GV.Left_Compensation);
|
||
|
|
}
|
||
|
|
void Move_Head_To_UP_Do(void)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotUpAngleValue);
|
||
|
|
|
||
|
|
}
|
||
|
|
void Move_Head_To_Down_Do(void)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotDownAngleValue);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void Calbrate_Robot_Positon(int Target_Angle)
|
||
|
|
{
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= 20) //100倍关系，大于10度
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = 0;
|
||
|
|
GV.RightMotor.Target_Velcity = 0;
|
||
|
|
}
|
||
|
|
else if (abs(GV.Robot_Angle - Target_Angle) < 9 * 100)
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 15, 0, 8, 1 / 0.880 * 101 * 10 );
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID(GV.Robot_Angle, Target_Angle, 25, 0, 10, 3 / 0.880 * 101 * 10 * 2);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle / 2;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle / 2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 终端上下移动，在洗舱里是抬头低头，这里是上下，直接沿用Tilt Up 和 TiltDown函数
|
||
|
|
* */
|
||
|
|
void TiltUp_Do()
|
||
|
|
{
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
||
|
|
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void TiltDown_Do()
|
||
|
|
{
|
||
|
|
|
||
|
|
// if (GV.ForceValue >= CV.PV.EndPushForceValue)
|
||
|
|
// {
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
||
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
|
||
|
|
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
|
||
|
|
void TiltHalt_Do()
|
||
|
|
{
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
||
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 拉毛函数控制
|
||
|
|
* */
|
||
|
|
void RougheningEndHALT_Do()
|
||
|
|
{
|
||
|
|
//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_OFF);
|
||
|
|
DMKE_Motors[0]->Command = 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void RougheningEndSwingClockWise_Do()
|
||
|
|
{
|
||
|
|
//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
|
||
|
|
DMKE_Motors[0]->TargetSpeedRpm = 2800;
|
||
|
|
|
||
|
|
|
||
|
|
DMKE_Motors[0]->TargetSpeedRpm = CV.DMK0_Speed;
|
||
|
|
|
||
|
|
|
||
|
|
DMKE_Motors[0]->Command = 1;
|
||
|
|
}
|
||
|
|
void RougheningEndSwingAnticlockWise_Do()
|
||
|
|
{
|
||
|
|
//GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
|
||
|
|
DMKE_Motors[0]->TargetSpeedRpm = 2800;
|
||
|
|
|
||
|
|
DMKE_Motors[0]->TargetSpeedRpm = CV.DMK0_Speed;
|
||
|
|
|
||
|
|
DMKE_Motors[0]->Command = -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void Wind_OFF()
|
||
|
|
{
|
||
|
|
GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_OFF);
|
||
|
|
}
|
||
|
|
void Wind_ON()
|
||
|
|
{
|
||
|
|
GF_BSP_GPIO_SetIO(Wind_IO_CTL, K_ON);
|
||
|
|
}
|
||
|
|
|
||
|
|
|