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.
365 lines
12 KiB
365 lines
12 KiB
6 days ago
|
/*
|
||
|
|
* robot_move_actions.c
|
||
|
|
*
|
||
|
|
* Created on: 2025年7月14日
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "robot_move_actions.h"
|
||
|
|
#include "fsm_state.h"
|
||
|
|
#include "fsm_state_control.h"
|
||
|
|
#include "MSP/msp_PID.h"
|
||
|
|
transition_t current_robot_move_state;
|
||
|
|
|
||
|
|
|
||
|
|
transition_state_t robot_halt_state={HALT_State_Enter,HALT_State_Do,HALT_State_Exit}; /* 停 */
|
||
|
|
transition_state_t robot_forwards_state={NULL,Forwards_State_Do,NULL}; /* 前 */
|
||
|
|
transition_state_t robot_backwards_state={NULL,Backwards_State_Do,NULL}; /* 后 */
|
||
|
|
transition_state_t robot_turn_left_state={NULL,TurnLeft_State_Do,NULL}; /* 左 */
|
||
|
|
transition_state_t robot_turn_right_state={NULL,TurnRight_State_Do,NULL}; /* 右 */
|
||
|
|
|
||
|
|
transition_state_t robot_move_vertical_task_forwards_state={NULL,Move_Vertical_Task_Forwards_Do,NULL}; /* 竖直纠偏前进 */
|
||
|
|
transition_state_t robot_move_vertical_task_backwards_state={NULL,Move_Vertical_Task_Backwards_Do,NULL}; /* 竖直纠偏后退 */
|
||
|
|
|
||
|
|
transition_state_t robot_move_horizontal_task_forwards_left_state={NULL,Move_Horizontal_Task_Forwards_Left_Do,NULL}; /* 水平纠偏前进 头朝左*/
|
||
|
|
transition_state_t robot_move_horizontal_task_backwards_left_state={NULL,Move_Horizontal_Task_Backwards_Left_Do,NULL};/* 水平纠偏后退 头朝左*/
|
||
|
|
transition_state_t robot_move_horizontal_task_forwards_right_state={NULL,Move_Horizontal_Task_Forwards_Right_Do,NULL}; /* 水平纠偏前进 头朝右*/
|
||
|
|
transition_state_t robot_move_horizontal_task_backwards_right_state={NULL,Move_Horizontal_Task_Backwards_Right_Do,NULL};/* 水平纠偏后退 头朝右*/
|
||
|
|
|
||
|
|
transition_state_t robot_move_horizontal_task_forwards_left_paint_state={NULL,Move_Horizontal_Task_Forwards_Left_Paint_Do,NULL}; /* 水平纠偏前进 头朝左*/
|
||
|
|
transition_state_t robot_move_horizontal_task_backwards_left_paint_state={NULL,Move_Horizontal_Task_Backwards_Left_Paint_Do,NULL};/* 水平纠偏后退 头朝左*/
|
||
|
|
transition_state_t robot_move_horizontal_task_forwards_right_paint_state={NULL,Move_Horizontal_Task_Forwards_Right_Paint_Do,NULL}; /* 水平纠偏前进 头朝右*/
|
||
|
|
transition_state_t robot_move_horizontal_task_backwards_right_paint_state={NULL,Move_Horizontal_Task_Backwards_Right_Paint_Do,NULL};/* 水平纠偏后退 头朝右*/
|
||
|
|
|
||
|
|
transition_state_t robot_move_head_to_left_enum_state={NULL,Move_Head_To_Left_Do,NULL}; /* 移动至头朝左 */
|
||
|
|
transition_state_t robot_move_head_to_up_enum_state={NULL,Move_Head_To_UP_Do,NULL}; /* 移动至头朝上 */
|
||
|
|
transition_state_t robot_move_head_to_right_enum_state={NULL,Move_Head_To_Right_Do,NULL}; /* 移动至头朝右 */
|
||
|
|
transition_state_t robot_move_head_to_down_enum_state={NULL,Move_Head_To_Down_Do,NULL}; /* 移动至头朝下 */
|
||
|
|
|
||
|
|
transition_state_t robot_move_head_to_up_add_adjust_enum_state={NULL,Move_Head_To_UP_Add_Adjust_Do,NULL};//移动到头朝上+竖直微调角度
|
||
|
|
transition_state_t robot_move_head_to_left_add_adjust_enum_state={NULL,Move_Head_To_Left_Add_Adjust_Do,NULL}; /* 移动至头朝左+竖直微调角度 */
|
||
|
|
transition_state_t robot_move_head_to_right_add_adjust_enum_state={NULL,Move_Head_To_Right_Add_Adjust_Do,NULL}; /* 移动至头朝右+竖直微调角度 */
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 声明最底层函数 */
|
||
|
|
void Move_Forwards_Do(int32_t Target_Angle);
|
||
|
|
void Move_Backwards_Do(int32_t Target_Angle);
|
||
|
|
void Calbrate_Robot_Positon(int Target_Angle);
|
||
|
|
|
||
|
|
int32_t* DHRoughening_Speed[10];
|
||
|
|
static double dletAngle = 0;
|
||
|
|
char IsRobotHaltSateChangedFlag = 0;
|
||
|
|
|
||
|
|
void Forwards_State_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Backwards_State_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed;
|
||
|
|
}
|
||
|
|
/* 向左转
|
||
|
|
*
|
||
|
|
* */
|
||
|
|
void TurnLeft_State_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = -CV.LeftTurnSpeed;
|
||
|
|
GV.RightMotor.Target_Velcity = CV.RightTurnSpeed;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 向右转
|
||
|
|
*
|
||
|
|
* */
|
||
|
|
void TurnRight_State_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = CV.LeftTurnSpeed;
|
||
|
|
GV.RightMotor.Target_Velcity = -CV.RightTurnSpeed;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 停止
|
||
|
|
* 零速停止，不关闭抱闸
|
||
|
|
* */
|
||
|
|
void HALT_State_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = 0;
|
||
|
|
GV.RightMotor.Target_Velcity = 0;
|
||
|
|
|
||
|
|
}
|
||
|
|
/* 竖直前进，保持目标角度 CV.RobotUpAngleValue
|
||
|
|
* Target_Angle: 0.01度
|
||
|
|
* */
|
||
|
|
void Move_Vertical_Task_Forwards_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Forwards_Do( (CV.RobotUpAngleValue + GV.PV.Vertical_Calibration) );
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 竖直后退，保持目标角度 CV.RobotUpAngleValue
|
||
|
|
* Target_Angle: 0.01度
|
||
|
|
* */
|
||
|
|
void Move_Vertical_Task_Backwards_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotUpAngleValue + GV.PV.Vertical_Calibration );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void HALT_State_Enter(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
void HALT_State_Exit(transition_t *p_this)
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 水平头朝右前进
|
||
|
|
* 带补偿
|
||
|
|
* */
|
||
|
|
void Move_Horizontal_Task_Forwards_Right_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
// Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation);
|
||
|
|
// 修改项V1.0：水平移动+左右补偿+水平微调
|
||
|
|
// Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration); //+补偿+水平微调
|
||
|
|
// 修改项V2.0：水平移动，共用竖直微调这一补偿项，即删除替换原来的水平微调。水平移动+左右补偿+竖直微调
|
||
|
|
Move_Forwards_Do(CV.RobotRightAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration); //+补偿+竖直微调
|
||
|
|
|
||
|
|
}
|
||
|
|
void Move_Horizontal_Task_Forwards_Right_Paint_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Forwards_Do(CV.RobotRightAngleValue);
|
||
|
|
}
|
||
|
|
/* 水平头朝右后退 ，向左退 左补偿生效
|
||
|
|
* 带补偿
|
||
|
|
* */
|
||
|
|
void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
// Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);
|
||
|
|
// Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration);
|
||
|
|
// 修改项V2.0：
|
||
|
|
Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration);
|
||
|
|
|
||
|
|
}
|
||
|
|
/**和拉毛区分开 防止左右补偿加到喷漆上**/
|
||
|
|
void Move_Horizontal_Task_Backwards_Right_Paint_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotRightAngleValue);
|
||
|
|
}
|
||
|
|
/* 水平头朝左前进
|
||
|
|
* 带补偿
|
||
|
|
* */
|
||
|
|
void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
// Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);
|
||
|
|
// Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Horizontal_Calibration);
|
||
|
|
// 修改项V2.0：
|
||
|
|
Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation + GV.PV.Vertical_Calibration);
|
||
|
|
|
||
|
|
}
|
||
|
|
/**和拉毛区分开 防止左右补偿加到喷漆上**/
|
||
|
|
void Move_Horizontal_Task_Forwards_Left_Paint_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Forwards_Do(CV.RobotLeftAngleValue);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 水平头朝左后退 向右退 右补偿
|
||
|
|
* 带补偿
|
||
|
|
* */
|
||
|
|
void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
// Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);
|
||
|
|
// Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Horizontal_Calibration);
|
||
|
|
// 修改项V2.0：
|
||
|
|
Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation + GV.PV.Vertical_Calibration);
|
||
|
|
|
||
|
|
}
|
||
|
|
void Move_Horizontal_Task_Backwards_Left_Paint_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Move_Backwards_Do(CV.RobotLeftAngleValue);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* 原地PID转向至目标角度 CV.RobotUpAngleValue
|
||
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
||
|
|
* */
|
||
|
|
void Move_Head_To_UP_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotUpAngleValue); // 原始版本，转到固定的0/90度那种
|
||
|
|
// Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)); // 加入竖直微调项的转动角度
|
||
|
|
}
|
||
|
|
|
||
|
|
void Move_Head_To_UP_Add_Adjust_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon((CV.RobotUpAngleValue + GV.PV.Vertical_Calibration)); // 加入竖直微调项的转动角度
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 原地PID转向至目标角度 CV.RobotDownAngleValue
|
||
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
||
|
|
* */
|
||
|
|
void Move_Head_To_Down_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotDownAngleValue);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* 原地PID转向至目标角度 CV.RobotLeftAngleValue
|
||
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
||
|
|
* */
|
||
|
|
void Move_Head_To_Left_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotLeftAngleValue);
|
||
|
|
// Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Move_Head_To_Left_Add_Adjust_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 原地PID转向至目标角度 CV.RobotRightAngleValue
|
||
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
||
|
|
* */
|
||
|
|
void Move_Head_To_Right_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotRightAngleValue);
|
||
|
|
// Calbrate_Robot_Positon(CV.RobotLeftAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Move_Head_To_Right_Add_Adjust_Do(transition_t *p_this)
|
||
|
|
{
|
||
|
|
Calbrate_Robot_Positon(CV.RobotRightAngleValue + GV.PV.Vertical_Calibration);// 加入竖直微调项的转动角度,用于水平
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
/*******************以下纠偏底层函数**********************************/
|
||
|
|
/*
|
||
|
|
* 底层前进，保持目标角度 纠偏 被调用
|
||
|
|
* Target_Angle: 0.1度
|
||
|
|
* */
|
||
|
|
int aaa=0;
|
||
|
|
int delt=0;
|
||
|
|
void Move_Forwards_Do(int32_t Target_Angle)
|
||
|
|
{
|
||
|
|
|
||
|
|
aaa=abs(GV.Robot_Angle - Target_Angle);
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= CV.PID_mid.PID_Angle) //// 误差在正负x度内
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) < CV.PID_low.PID_Angle) //// 误差在正负0.3度内
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_low.Kp, CV.PID_low.Ki, CV.PID_low.Kd, 5);
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity =GV.Robot_Move_Speed + dletAngle ;
|
||
|
|
}
|
||
|
|
else //中角度 low-mid 0.3-x
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_mid.Kp, CV.PID_mid.Ki, CV.PID_mid.Kd, 10);
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
else //>1
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_high.Kp, CV.PID_high.Ki, CV.PID_high.Kd,40);
|
||
|
|
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
||
|
|
}
|
||
|
|
delt=dletAngle;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* 后退，保持目标角度 纠偏
|
||
|
|
* Target_Angle: 0.01度
|
||
|
|
* */
|
||
|
|
void Move_Backwards_Do(int32_t Target_Angle)
|
||
|
|
{
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= CV.PID_mid.PID_Angle) //// 误差在正负4度内
|
||
|
|
{
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) < CV.PID_low.PID_Angle) //// 误差在正负1度内
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_low.Kp, CV.PID_low.Ki, CV.PID_low.Kd, 5);
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed - dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Robot_Move_Speed + dletAngle ;
|
||
|
|
}
|
||
|
|
else /*1-4*/
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_mid.Kp, CV.PID_mid.Ki, CV.PID_mid.Kd, 10);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
else /*大于4*/
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle,
|
||
|
|
CV.PID_high.Kp, CV.PID_high.Ki, CV.PID_high.Kd, 40);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* 原地PID转向至目标角度
|
||
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
||
|
|
* */
|
||
|
|
void Calbrate_Robot_Positon(int Target_Angle)
|
||
|
|
{
|
||
|
|
|
||
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= 50) //误差在正负1度内
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = 0;
|
||
|
|
GV.RightMotor.Target_Velcity = 0;
|
||
|
|
}
|
||
|
|
else if (abs(GV.Robot_Angle - Target_Angle) <= 1000) //误差在正负1度内 1-5°
|
||
|
|
{
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 1, 0,0.5, 10);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ; /* 换道回正or转向时，最后的5度内，转速降低 */
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle;
|
||
|
|
}
|
||
|
|
else//大于2°
|
||
|
|
{
|
||
|
|
|
||
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 2, 0, 0.5, 50);
|
||
|
|
GV.LeftMotor.Target_Velcity = -dletAngle;/* 换道回正or转向时，最后的5度内，转速降低 */
|
||
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* 车体速度选择 具体车速在配置结构体CV 0.01m/min */
|
||
|
|
double GetVehicleSpeed(double speed_selection)
|
||
|
|
{
|
||
|
|
|
||
|
|
if(speed_selection==0)
|
||
|
|
{
|
||
|
|
return 1.0;
|
||
|
|
}
|
||
|
|
return CV.MAXSpeed_m_per_min*speed_selection/20.0;
|
||
|
|
|
||
|
|
}
|