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.
286 lines
8.8 KiB
286 lines
8.8 KiB
/*
|
|
* Automatic_task_Control.c
|
|
*
|
|
* Created on: 2026年4月13日
|
|
* Author: SHI
|
|
*/
|
|
#include "Automatic_task_Control.h"
|
|
|
|
static uint32_t height_storage = 0; //总高多少cm？
|
|
static const uint32_t height_ending = 100; //作业下限多少cm？
|
|
|
|
static uint8_t last_state_DDM = 0;
|
|
static uint8_t current_state_DDM = 0;//电子罗盘4个方向状态
|
|
static uint8_t count_DDM = 0;//方向状态 变化计数
|
|
static uint8_t flag_ChangeLine_complete = 1;//换道完成标志
|
|
static float current_level = 0;//开始换道时的作业高度
|
|
|
|
LaneChangeControlSTATE HorizontalLaneChangeState; /*当前换道处于开始或者结束*/
|
|
|
|
static int get_state_DDM();
|
|
static int Auto_steer_do(float target_angle);
|
|
|
|
/*
|
|
* 自动作业（圆罐环境，从上往下，从下看 头朝左）
|
|
* 1.根据电子罗盘状态，经历东南西北4个方向状态 判断为一圈作业循环
|
|
* 2.走完一圈执行 换道（换道完重置方向状态）
|
|
* 3.根据罐体高度和拉线长度，判断是否到达圆罐底部，作业完成
|
|
*/
|
|
void Automatic_task_Control()
|
|
{
|
|
height_storage = GV.PV.Height_Storage * 100;
|
|
if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED
|
|
||Get_BIT(SystemErrorCode, ComError_DDM360B) == DISCONNECTED
|
|
||Get_BIT(SystemErrorCode, ComError_wire_sensor) == DISCONNECTED)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;//安卓屏幕 模式选择错误
|
|
}
|
|
if (GV.PV.RunMode == Move_Manual)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;
|
|
}
|
|
|
|
if( GV.Wire_lenth > (height_storage - height_ending))//到达底部
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;
|
|
}
|
|
|
|
if(current_state_DDM !=0) {last_state_DDM = current_state_DDM;}// 只有上次状态有效时才保存
|
|
current_state_DDM = get_state_DDM();// 更新状态
|
|
if(current_state_DDM != last_state_DDM) {count_DDM ++;} // 状态变化时计数
|
|
|
|
if(count_DDM >= 5)//换道
|
|
{
|
|
Auto_ChangeLine_Control(0);
|
|
}
|
|
else//绕圈
|
|
{
|
|
Auto_steer_moveforward_do(-90 + GV.PV.Vertical_Calibration/100);//垂直校准，正抬头，负低头
|
|
}
|
|
}
|
|
|
|
//电子罗盘 北东南西 对应4个状态
|
|
static int get_state_DDM()
|
|
{
|
|
if(GV.RobotAngle_DDM >5*100 && GV.RobotAngle_DDM <90*100) //[5,90]
|
|
{return 1;}
|
|
else if(GV.RobotAngle_DDM >95*100 && GV.RobotAngle_DDM <180*100) //[95,180]
|
|
{return 2;}
|
|
else if(GV.RobotAngle_DDM >185*100 && GV.RobotAngle_DDM <270*100) //[185,270]
|
|
{return 3;}
|
|
else if(GV.RobotAngle_DDM >275*100 && GV.RobotAngle_DDM <360*100) //[275,360]
|
|
{return 4;}
|
|
return 0;
|
|
}
|
|
|
|
void Refresh_MK32_State()
|
|
{
|
|
if (P_MK32->CH4_SA == 0)//更新换道状态
|
|
{
|
|
HorizontalLaneChangeState = Lane_Change_Start;//准备开始换道
|
|
return;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* 后退换道（从下看 头朝左->头朝上->头朝左，可修改）
|
|
* 1.距离差大于5，先调整方向至0°，再后退
|
|
* 2.距离差大于2小于5，慢速后退
|
|
* 3.调整方向至90°
|
|
*/
|
|
// 0-自动作业换道; 1-手动拨杆换道
|
|
void Auto_ChangeLine_Control(int flag)//换道(后退)
|
|
{
|
|
if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED
|
|
||Get_BIT(SystemErrorCode, ComError_wire_sensor) == DISCONNECTED)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;//安卓屏幕 模式选择错误
|
|
}
|
|
if (GV.PV.RunMode == Move_Manual)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;//安卓屏幕 模式选择错误
|
|
}
|
|
if (flag == 1)
|
|
{
|
|
|
|
if (HorizontalLaneChangeState != Lane_Change_Start)
|
|
{
|
|
fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
return;//换道完 拨杆未复位
|
|
}
|
|
}
|
|
|
|
height_storage = GV.PV.Height_Storage * 100;
|
|
float current_pos = height_storage - GV.Wire_lenth;//当前实时高度
|
|
if(flag == 1 && flag_ChangeLine_complete == 0)//手动拨杆换道时，上一个自动换道未完成
|
|
{
|
|
flag_ChangeLine_complete = 2;
|
|
current_level = current_pos;
|
|
}
|
|
if(flag == 0 && flag_ChangeLine_complete == 2)//自动作业换道时，上一个手动拨杆换道未完成
|
|
{
|
|
flag_ChangeLine_complete = 0;
|
|
current_level = current_pos;
|
|
}
|
|
if(flag_ChangeLine_complete == 1)//上一个换道完成
|
|
{
|
|
flag_ChangeLine_complete = 0;
|
|
current_level = current_pos;//只记录一次,当前作业高度
|
|
}
|
|
|
|
float Target_pos = current_level - GV.LaneChangeDistance;// 目标后退高度
|
|
float delta_pos = current_pos - Target_pos;
|
|
|
|
if(delta_pos > 5)//1.大差距，先调整方向再后退
|
|
{
|
|
Auto_steer_moveback_do(0);
|
|
}
|
|
else if(delta_pos > 2 && delta_pos <= 5)//2.慢速后退
|
|
{
|
|
GV.LeftMotor.Target_Velcity = -GV.Robot_Move_Speed/2 ;
|
|
GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed/2 ;
|
|
}
|
|
else//3.调整方向
|
|
{
|
|
static uint32_t timeout_count = 0;
|
|
if( Auto_steer_do(-90) == 1 )
|
|
{
|
|
if(flag == 1)
|
|
{
|
|
HorizontalLaneChangeState = Lane_Change_Stop;
|
|
}
|
|
count_DDM = 0;
|
|
flag_ChangeLine_complete = 1;
|
|
timeout_count = 0;
|
|
}
|
|
// else if( ++timeout_count > 5000 )//10s超时判断
|
|
// {
|
|
// count_DDM = 0;
|
|
// flag_ChangeLine_complete = 1;
|
|
// timeout_count = 0;
|
|
// }
|
|
}
|
|
}
|
|
|
|
//先原地转向，再前进// 目标角度 1°
|
|
void Auto_steer_moveforward_do(float TargetAngle)//1°
|
|
{
|
|
float Target_Angle = TargetAngle * 100;//0.01°
|
|
float current_Angle = GV.Robot_Angle;
|
|
float delta_Angle = current_Angle - Target_Angle;
|
|
|
|
double dletSpeed = Angle_Tune_PID( current_Angle, Target_Angle, 0.01, 0, 0.01, 2);
|
|
if(abs(delta_Angle) > 2*100)
|
|
{ //double copysign(double x, double y); //结果具有x绝对值大小和y的符号
|
|
GV.LeftMotor.Target_Velcity = -1*copysign(CV.LeftTurnSpeed, delta_Angle);
|
|
GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
|
|
}
|
|
else
|
|
{
|
|
GV.LeftMotor.Target_Velcity = (GV.Robot_Move_Speed - dletSpeed*1);
|
|
GV.RightMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletSpeed*1);
|
|
}
|
|
|
|
}
|
|
//先原地转向，再后退// 目标角度 1°
|
|
void Auto_steer_moveback_do(float TargetAngle)//1°
|
|
{
|
|
float Target_Angle = TargetAngle *100;//0.01°
|
|
float current_Angle = GV.Robot_Angle;
|
|
float delta_Angle = current_Angle - Target_Angle;
|
|
|
|
double dletSpeed = Angle_Tune_PID( current_Angle, Target_Angle, 0.02, 0, 0.01, 2);
|
|
if(abs(delta_Angle) > 5*100)
|
|
{ //double copysign(double x, double y); //结果具有x绝对值大小和y的符号
|
|
GV.LeftMotor.Target_Velcity = -1*copysign(CV.LeftTurnSpeed, delta_Angle);
|
|
GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
|
|
}
|
|
else
|
|
{
|
|
GV.LeftMotor.Target_Velcity = -(GV.Robot_Move_Speed + dletSpeed*1);
|
|
GV.RightMotor.Target_Velcity = GV.Robot_Move_Speed - dletSpeed*1;
|
|
}
|
|
|
|
}
|
|
//原地转向// 目标角度 1°
|
|
static int Auto_steer_do(float TargetAngle)//1°
|
|
{
|
|
float Target_Angle = TargetAngle *100;
|
|
float current_Angle = GV.Robot_Angle;
|
|
float delta_Angle = current_Angle - Target_Angle;
|
|
|
|
if(abs(delta_Angle) > 10*100) //大角度快速转
|
|
{ //double copysign(double x, double y); //结果具有x绝对值大小和y的符号
|
|
GV.LeftMotor.Target_Velcity = -1*copysign(CV.LeftTurnSpeed, delta_Angle);
|
|
GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed, delta_Angle);
|
|
}
|
|
else if(abs(delta_Angle) > 2*100)//小角度慢速转
|
|
{
|
|
GV.LeftMotor.Target_Velcity = -1*copysign(CV.LeftTurnSpeed/2, delta_Angle);
|
|
GV.RightMotor.Target_Velcity = -1*copysign(CV.RightTurnSpeed/2, delta_Angle);
|
|
}
|
|
else // <小角度,停止
|
|
{
|
|
GV.LeftMotor.Target_Velcity = 0;
|
|
GV.RightMotor.Target_Velcity = 0;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
uint8_t blowers_pwm_last = 0;
|
|
int SetPWM_Auto()//
|
|
{
|
|
uint8_t blowers_pwm_base = 0;//旋钮pwm，增加默认值，否则遥控连接之前，获取旋钮速度输出为50
|
|
int8_t blowers_pwm_PID = 0;//自动pwm
|
|
|
|
if (P_MK32->CH6_SC == -1000)//自动调整风速
|
|
{
|
|
//根据压强大小 线性调整PWM，风机转速
|
|
//压强数值输出范围是 -100 ~ 100 Kpa,//压强减小，blowers_pwm_Kp增大（>0）
|
|
|
|
float Target_force = 3.2;
|
|
uint8_t base_pwm = 41;// 41对应3.2；45对应3.8Kpa；
|
|
float current_force = -1*GV.ForceValue ;
|
|
float deltForce = current_force - Target_force;
|
|
int detapwm = 0;
|
|
if(deltForce < 0)
|
|
{
|
|
detapwm = Angle_Tune_PID( current_force, Target_force, 20, 0, 16, 100);
|
|
}
|
|
else if(deltForce < 0.5)
|
|
{
|
|
detapwm = Angle_Tune_PID( current_force, Target_force, 6, 0, 10, 100);//
|
|
}
|
|
else
|
|
{
|
|
detapwm = Angle_Tune_PID( current_force, Target_force, 10, 0, 8, 100);
|
|
}
|
|
|
|
blowers_pwm_PID = base_pwm - detapwm;
|
|
if(blowers_pwm_PID < 40) {blowers_pwm_PID = 40;}//下限
|
|
if(blowers_pwm_PID > 100) {blowers_pwm_PID = 100;}//上限
|
|
// blowers_pwm_base = 0;
|
|
}
|
|
else//手动调节
|
|
{
|
|
if(Get_BIT(SystemErrorCode, ComError_MK32_SBus) == CONNECTED)
|
|
{
|
|
blowers_pwm_base = (uint8_t)(abs(P_MK32->CH10_LD1 + 1000) / 200.0f * 10);//0-100/*获取旋钮速度*/
|
|
// blowers_pwm_PID = 0;
|
|
blowers_pwm_last = blowers_pwm_base;
|
|
}
|
|
else//遥控通讯中断，风机不能停
|
|
{
|
|
blowers_pwm_base = blowers_pwm_last;
|
|
}
|
|
}
|
|
|
|
int blowers_pwm_total = blowers_pwm_base + blowers_pwm_PID;
|
|
return (blowers_pwm_total);//pwm > 90, 风速最大
|
|
}
|
|
|