private void Adjust_X(ControlPort controlport, UrgPort urgport)
{
// 沿用调整 A 过程时的数据
// while (!GetUrgData(urgport)) ;
CON_parameter.TimeFor_control = CON_parameter.TimeFor_0x70 + CON_parameter.TimeFor_URG + CON_parameter.TimeFor_process;
Initial_PID_parameter(0.0, 0.0, 0.0);
while (true)
{
double current = currPoint.UrgB;
double target = prevPoint.UrgB;
if (Math.Abs(current - target) < CON_parameter.X_Error)
{
return;
}
double adjustX = PIDcontroller1(current, target);
int xSpeed = (int)(adjustX / CON_parameter.TimeFor_control);
if (xSpeed == 0)
{
return;
}
urgport.GetUrgData();
//controlport.MoveControl_0x70(xSpeed, 0, 0);
System.Threading.Thread.Sleep(CON_parameter.TimeFor_0x70);
while (!GetUrgData(urgport))
{
;
}
}
}
////////////////////////////////////////// public method ////////////////////////////////////////////////
/// <summary>
/// 对小车当前位置进行校准。
/// </summary>
/// <param name="controlport">控制口</param>
/// <param name="urgport">激光雷达口</param>
/// <param name="point">关键点</param>
public void Start(ControlPort controlport, UrgPort urgport, KeyPoint point)
{
prevPoint = point;
// 粗调
CON_parameter.Fit_Percent = 0.05;
CON_parameter.A_Error = 50;
CON_parameter.X_Error = 10;
CON_parameter.Y_Error = 10;
CON_parameter.TimeFor_0x86 = 100;
CON_parameter.TimeFor_URG = 10;
CON_parameter.TimeFor_process = 2;
CON_parameter.TimeFor_0x70 = 50 - CON_parameter.TimeFor_URG - CON_parameter.TimeFor_process;
Adjust_A(controlport, urgport);
Adjust_X(controlport, urgport);
Adjust_Y(controlport, urgport);
// 细调
CON_parameter.Fit_Percent = 0.02;
CON_parameter.A_Error = 20;
CON_parameter.X_Error = 5;
CON_parameter.Y_Error = 5;
CON_parameter.TimeFor_0x86 = 100;
CON_parameter.TimeFor_URG = 10;
CON_parameter.TimeFor_process = 2;
CON_parameter.TimeFor_0x70 = 50 - CON_parameter.TimeFor_URG - CON_parameter.TimeFor_process;
Adjust_A(controlport, urgport);
Adjust_X(controlport, urgport);
Adjust_Y(controlport, urgport);
}
private bool GetUrgData(UrgPort urgport)
{
// 获取原始数据
if (!urgport.GetUrgData())
{
return(false);
}
urgport.MidFilter();
// 获取拟合误差
double sumDistance = 0;
double N = 0;
for (int i = 0; i < urgport.urgData.distance.Count; i++)
{
if (urgport.urgData.distance[i] == 0)
{
continue;
}
N++;
sumDistance += urgport.urgData.distance[i];
}
CON_parameter.Fit_Error = sumDistance / N * CON_parameter.Fit_Percent;
// 分割与拟合
Pole2Rectangular(urgport.urgData.distance, urgport.urgData.StartAngle, urgport.urgData.AnglePace);
urgGroups = new List <List <UrgPoint> >();
CutGroup_UrgPoint(urgPoints);
List <UrgPoint> linePoints = GetGroup_UrgPoint();
Fit_UrgPoint(linePoints);
return(true);
}
/// <summary>
/// 得到当前位置的超声波和墙壁信息，数据在 currPoint 中。
/// </summary>
/// <param name="controlport">控制口</param>
/// <param name="urgport">激光雷达口</param>
/// <returns></returns>
public bool GetCurrentPoint(ControlPort controlport, UrgPort urgport)
{
CON_parameter.Fit_Percent = 0.01;
CON_parameter.TimeFor_0x86 = 100;
urgport.GetUrgData();
//if (!GetSonicData(controlport)) { return false; }
System.Threading.Thread.Sleep(100);
currPoint.UltraSonicL = (controlport.UltraSonic.Head_L_Y + controlport.UltraSonic.Tail_L_Y) / 2;
currPoint.UltraSonicR = (controlport.UltraSonic.Head_R_Y + controlport.UltraSonic.Tail_R_Y) / 2;
return(GetUrgData(urgport));
}
private void Adjust_A(ControlPort controlport, UrgPort urgport)
{
urgport.GetUrgData();
System.Threading.Thread.Sleep(100);
while (!GetUrgData(urgport))
{
;
}
CON_parameter.TimeFor_control = CON_parameter.TimeFor_0x70 + CON_parameter.TimeFor_URG + CON_parameter.TimeFor_process;
Initial_PID_parameter(0.0, 0.0, 0.0);
while (true)
{
double current = Math.Atan(currPoint.UrgK) * 18000 / Math.PI;
double target = Math.Atan(prevPoint.UrgK) * 18000 / Math.PI;
if (Math.Abs(current - target) <= CON_parameter.A_Error)
{
return;
}
double adjustA = PIDcontroller1(current, target);
int aSpeed = (int)(adjustA / CON_parameter.TimeFor_control);
if (aSpeed == 0)
{
return;
}
//controlport.MoveControl_0x70(0, 0, aSpeed);
urgport.GetUrgData();
System.Threading.Thread.Sleep(CON_parameter.TimeFor_0x70);
while (!GetUrgData(urgport))
{
;
}
}
}
private void Adjust_Y(ControlPort controlport, UrgPort urgport)
{
while (!GetSonicData(controlport))
{
;
}
CON_parameter.TimeFor_control = CON_parameter.TimeFor_0x70 + CON_parameter.TimeFor_0x86 + CON_parameter.TimeFor_process;
Initial_PID_parameter(0.0, 0.0, 0.0);
bool NearLeft = prevPoint.UltraSonicL >= prevPoint.UltraSonicR;
while (true)
{
double current = NearLeft ? currPoint.UltraSonicL : currPoint.UltraSonicR;
double target = NearLeft ? prevPoint.UltraSonicL : prevPoint.UltraSonicR;
if (Math.Abs(current - target) < CON_parameter.Y_Error)
{
return;
}
double adjustY = PIDcontroller1(current, target);
int ySpeed = (int)(adjustY / (CON_parameter.TimeFor_control));
if (ySpeed == 0)
{
return;
}
//controlport.MoveControl_0x70(ySpeed, 0, 0);
System.Threading.Thread.Sleep(CON_parameter.TimeFor_0x70);
while (!GetSonicData(controlport))
{
;
}
}
}
/// <summary>
/// 对 AGV 小车的起始点进行校准。
/// </summary>
/// <param name="controlport">控制口</param>
/// <param name="urgport">激光雷达口</param>
/// <param name="point">起始关键点</param>
public void Correct_1st(ControlPort controlport, UrgPort urgport, KeyPoint point)
{
}
