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.
335 lines
11 KiB
335 lines
11 KiB
/*
|
|
* 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 "paint_gun_action.h"
|
|
#include "MSP/msp_PID.h"
|
|
|
|
transition_t current_robot_move_state;
|
|
transition_t current_robot_automove_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_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_Automatic_task_state = {NULL, Move_Automatic_task_Do, NULL};//自动作业
|
|
transition_state_t robot_Auto_ChangeLine_state = {NULL, Move_Auto_ChangeLine_Do, NULL};//换道
|
|
transition_state_t robot_move_horizontal_task_forwards_state ={NULL,Move_Horizontal_Task_Forwards_Do,NULL};/* 水平纠偏前进 */
|
|
transition_state_t robot_move_horizontal_task_backwards_state ={NULL,Move_Horizontal_Task_Backwards_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);
|
|
|
|
|
|
char IsRobotHaltSateChangedFlag = 0;
|
|
|
|
void Move_Automatic_task_Do(transition_t *p_this)
|
|
{
|
|
Automatic_task_Control();
|
|
}
|
|
void Move_Auto_ChangeLine_Do(transition_t *p_this)
|
|
{
|
|
Auto_ChangeLine_Control(1);
|
|
}
|
|
|
|
void Move_Horizontal_Task_Forwards_Do(transition_t *p_this)
|
|
{
|
|
if(GV.Robot_Angle >= 0)//头朝右
|
|
{
|
|
Auto_steer_moveforward_do(90 - GV.PV.Vertical_Calibration/100);
|
|
}
|
|
else//头朝左
|
|
{
|
|
Auto_steer_moveforward_do(-90 + GV.PV.Vertical_Calibration/100);
|
|
}
|
|
|
|
}
|
|
void Move_Horizontal_Task_Backwards_Do(transition_t *p_this)
|
|
{
|
|
if(GV.Robot_Angle >= 0)//头朝右
|
|
{
|
|
Auto_steer_moveback_do(90 + GV.PV.Vertical_Calibration/100);
|
|
}
|
|
else//头朝左
|
|
{
|
|
Auto_steer_moveback_do(-90 - GV.PV.Vertical_Calibration/100);
|
|
}
|
|
}
|
|
|
|
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.LeftTurnSpeed;
|
|
}
|
|
|
|
/* 向右转 */
|
|
void TurnRight_State_Do(transition_t *p_this)
|
|
{
|
|
GV.LeftMotor.Target_Velcity =CV.RightTurnSpeed;
|
|
GV.RightMotor.Target_Velcity = CV.RightTurnSpeed;
|
|
}
|
|
|
|
/* 停止
|
|
* 零速停止，不关闭抱闸
|
|
* */
|
|
void HALT_State_Do(transition_t *p_this)
|
|
{
|
|
GV.LeftMotor.Target_Velcity = 0;
|
|
GV.RightMotor.Target_Velcity = 0;
|
|
|
|
// if (IsRobotHaltSateChangedFlag == 1)//机器人为运动状态
|
|
// {
|
|
// if (current_paintgun_state.p_state == &paintgun_on_state) //停车看喷枪状态 开了关喷枪计时停车
|
|
// {
|
|
// fsm_state_set(¤t_paintgun_state, &paintgun_off_state);
|
|
// timer_handler_3.start_timer = 1; //开始计时
|
|
// return;
|
|
// }
|
|
// if (CompareTimer(
|
|
// 600 * CV.Paint_Gun_Shutdown_Distance / GetVehicleSpeed( speed_selection),
|
|
// &timer_handler_3)) //喷枪开 计时停 关 直接停
|
|
// {
|
|
// IsRobotHaltSateChangedFlag = 0;
|
|
// GV.LeftMotor.Target_Velcity = 0;
|
|
// GV.RightMotor.Target_Velcity = 0;
|
|
// }
|
|
// }else
|
|
// {
|
|
// GV.LeftMotor.Target_Velcity = 0;
|
|
// GV.RightMotor.Target_Velcity = 0;
|
|
// IsRobotHaltSateChangedFlag=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);
|
|
}
|
|
/* 水平头朝右后退 ，向左退 左补偿生效
|
|
* 带补偿
|
|
* */
|
|
void Move_Horizontal_Task_Backwards_Right_Do(transition_t *p_this)
|
|
{
|
|
Move_Backwards_Do(CV.RobotRightAngleValue + GV.Left_Compensation);
|
|
}
|
|
/* 水平头朝左前进
|
|
* 带补偿
|
|
* */
|
|
void Move_Horizontal_Task_Forwards_Left_Do(transition_t *p_this)
|
|
{
|
|
Move_Forwards_Do(CV.RobotLeftAngleValue + GV.Left_Compensation);
|
|
}
|
|
|
|
|
|
/* 水平头朝左后退 向右退 右补偿
|
|
* 带补偿
|
|
* */
|
|
void Move_Horizontal_Task_Backwards_Left_Do(transition_t *p_this)
|
|
{
|
|
Move_Backwards_Do(CV.RobotLeftAngleValue - GV.Right_Compensation);
|
|
}
|
|
|
|
|
|
/* 原地PID转向至目标角度 CV.RobotUpAngleValue
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
|
* */
|
|
void Move_Head_To_UP_Do(transition_t *p_this)
|
|
{
|
|
Calbrate_Robot_Positon(CV.RobotUpAngleValue);
|
|
|
|
}
|
|
|
|
/* 原地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);
|
|
}
|
|
|
|
/* 原地PID转向至目标角度 CV.RobotRightAngleValue
|
|
* Target_Angle： 目标角度 （单位 0.01 度）
|
|
* */
|
|
void Move_Head_To_Right_Do(transition_t *p_this)
|
|
{
|
|
Calbrate_Robot_Positon(CV.RobotRightAngleValue);
|
|
}
|
|
|
|
|
|
/*******************以下纠偏底层函数**********************************/
|
|
|
|
double dletAngle;
|
|
double delt;
|
|
|
|
/* 竖直面 */
|
|
/* * 底层前进，保持目标角度 纠偏 被调用
|
|
* Target_Angle: 0.01度
|
|
* */
|
|
|
|
void Move_Forwards_Do(int32_t Target_Angle)
|
|
{
|
|
if (abs(GV.Robot_Angle - Target_Angle) <= 400) //// 误差在正负4度内,行进中校正
|
|
{
|
|
if (abs(GV.Robot_Angle - Target_Angle) < 100) //// 误差在正负1度内
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.01, 0, 0.01, 1);
|
|
}
|
|
else //1-4°
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0, 0.01, 4);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);
|
|
|
|
}
|
|
GV.LeftMotor.Target_Velcity = GV.Robot_Move_Speed - dletAngle ;
|
|
GV.RightMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletAngle) ;//注意电机方向
|
|
}
|
|
else //大于4°,原地转向校正
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 6);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.03, 0, 0.01, 3);
|
|
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) <= 400) //// 误差在正负4度内
|
|
{
|
|
if (abs(GV.Robot_Angle - Target_Angle) < 100) //// 误差在正负1度内
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0, 0.01, 2);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.01, 0, 0.01, 1);
|
|
}
|
|
else /*1-4*/
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0,0.01, 4);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.02, 0,0.01, 2);
|
|
}
|
|
GV.LeftMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletAngle) ;
|
|
GV.RightMotor.Target_Velcity = (GV.Robot_Move_Speed - dletAngle);//注意电机方向
|
|
}
|
|
else /*大于4*/
|
|
{
|
|
// dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 8);
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.03, 0, 0.01, 3);
|
|
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) <= 200) //误差在正负1度内 1-2°
|
|
{
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.04, 0,0.01, 4);
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
|
GV.RightMotor.Target_Velcity = dletAngle;
|
|
}
|
|
else//大于2°
|
|
{
|
|
dletAngle = Angle_Tune_PID((double)GV.Robot_Angle, Target_Angle, 0.06, 0, 0.01, 8);
|
|
GV.LeftMotor.Target_Velcity = -dletAngle ;
|
|
GV.RightMotor.Target_Velcity = dletAngle ;
|
|
}
|
|
}
|
|
|
|
int32_t GetVehicleSpeed(float speed_selection)
|
|
{
|
|
//(1r/min)360*81*100/360/60=135 //减速比81
|
|
// 1r=3.14*180mm/1000 =0.5652m
|
|
// 1m/min=135/0.5652= 238.85
|
|
// return (int32_t)(238.85*speed_selection);
|
|
|
|
float speed_m_per_min = speed_selection;
|
|
return (int32_t) (speed_m_per_min *CV.pulse_Per_Circle * CV.wheel_Reduction_Ratio / 60.0f
|
|
/ (3.14159265f * CV.wheel_Diameter_m));
|
|
}
|
|
|