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);
}
/// <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 method ////////////////////////////////////////////////
private bool GetSonicData(ControlPort controlport)
{
if (!controlport.GetUltraSonicData_0x86())
{
return(false);
}
System.Threading.Thread.Sleep(CON_parameter.TimeFor_0x86);
if (controlport.Receiving)
{
return(false);
}
currPoint.UltraSonicL = controlport.UltraSonic.Tail_L_Y;
currPoint.UltraSonicR = controlport.UltraSonic.Tail_R_Y;
return(true);
}
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)
{
}