////////////////////////////////////////// public method ///////////////////////////////////////////////
public void Start(int xSpeed, KeyPoint point, TH_SendCommand TH_command)
{
// 目标信息
config.TargetL = point.UrgL;
config.TargetR = point.UrgR;
while (true)
{
// 判断结束信息
// 获取两侧距离
GetSideDistance();
// 按要求调整(要考虑没收到数据)
if (config.KeepL)
{
double current = Math.Atan(config.CurrentL - config.PreviousL) / (xSpeed * TH_SendCommand.TH_data.TimeForControl / 1000);
double target = Math.Atan((config.CurrentL * Math.Cos(current) - config.TargetL) / config.TrackLength);
if (Math.Abs(current - target) < config.Error_A)
{
return;
}
double adjust = PIDcontroller1(current, target);
int aSpeed = (int)(adjust * 100);
TH_command.AGV_MoveControl_0x70(xSpeed, 0, aSpeed);
}
}
}
public void GetCurrentPoint(TH_SendCommand TH_command)
{
CON_parameter.Fit_Percent = 0.01;
GetUrgKB();
GetSonicData(TH_command);
}
public void Correct_1st(TH_SendCommand TH_command, KeyPoint point)
{
CON_parameter.FirstKeyPoint = true;
CON_parameter.LeftWall = true;
prevPoint = point;
// 粗调
CON_parameter.Fit_Percent = 0.05;
CON_parameter.A_Error = 50;
CON_parameter.X_Error = 10;
CON_parameter.Y_Error = 5;
Adjust_A(TH_command);
Adjust_X(TH_command);
Adjust_Y(TH_command);
// 细调
CON_parameter.Fit_Percent = 0.02;
CON_parameter.A_Error = 20;
CON_parameter.X_Error = 5;
CON_parameter.Y_Error = 2;
Adjust_A(TH_command);
Adjust_X(TH_command);
Adjust_Y(TH_command);
}
////////////////////////////////////////// public method ///////////////////////////////////////////////
public void Start(TH_SendCommand TH_command, KeyPoint point)
{
CON_parameter.FirstKeyPoint = false;
prevPoint = point;
// 粗调
CON_parameter.Fit_Percent = 0.05;
CON_parameter.A_Error = 50;
CON_parameter.X_Error = 10;
CON_parameter.Y_Error = 5;
Adjust_A(TH_command);
Adjust_X(TH_command);
Adjust_Y(TH_command);
// 细调
CON_parameter.Fit_Percent = 0.02;
CON_parameter.A_Error = 20;
CON_parameter.X_Error = 5;
CON_parameter.Y_Error = 2;
Adjust_A(TH_command);
Adjust_X(TH_command);
Adjust_Y(TH_command);
}
////////////////////////////////////////// private method ///////////////////////////////////////////////
private void GetSonicData(TH_SendCommand TH_command)
{
while (!TH_command.MeasureUltraSonic_0x86())
{
;
}
TH_command.StopSendCommand_Sonic_0x86();
currPoint.UltraSonicL = TH_SendCommand.TH_data.Tail_L_Y;
currPoint.UltraSonicR = TH_SendCommand.TH_data.Tail_R_Y;
}
private void Adjust_Y(TH_SendCommand TH_command)
{
Initial_PID_parameter(1.0, 0.0, 0.0);
GetSonicData(TH_command);
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)
{
break;
}
// 得到调整量
double adjustY = PIDcontroller1(current, target);
int ySpeed = (int)(adjustY * 100 / TH_SendCommand.TH_data.TimeForControl / 2);
if (ySpeed == 0)
{
break;
}
// 限幅
if (ySpeed > 100)
{
ySpeed = 100;
}
if (ySpeed < -100)
{
ySpeed = -100;
}
// 控制
TH_command.AGV_MoveControl_0x70(0, ySpeed, 0);
// 更新数据
GetSonicData(TH_command);
}
}
private void Adjust_A_1st(TH_SendCommand TH_command)
{
Initial_PID_parameter(1.0, 0.0, 0.0);
GetUrgKB();
while (true)
{
// 比较精度
double current = currPoint.UrgExtraK * 100;
double target = prevPoint.UrgExtraK * 100;
if (Math.Abs(current - target) <= CON_parameter.A_Error)
{
return;
}
// 获取控制速度
double adjustA = PIDcontroller1(current, target);
int aSpeed = (int)(-adjustA * 100 / TH_SendCommand.TH_data.TimeForControl);
if (aSpeed == 0)
{
return;
}
// 限幅
if (aSpeed > 200)
{
aSpeed = 200;
}
if (aSpeed < -200)
{
aSpeed = -200;
}
// 控制
TH_command.AGV_MoveControl_0x70(0, 0, aSpeed);
// 更新数据
GetUrgData();
}
}
private void Adjust_Y_1st(TH_SendCommand TH_command)
{
Initial_PID_parameter(1.0, 0.0, 0.0);
while (true)
{
// 比较精度
double current = currPoint.UrgExtraB;
double target = prevPoint.UrgExtraB;
if (Math.Abs(current - target) < CON_parameter.Y_Error)
{
return;
}
// 获得输出
double adjustY = PIDcontroller1(current, target);
int ySpeed = (int)(-adjustY * 100 / TH_SendCommand.TH_data.TimeForControl);
if (ySpeed == 0)
{
return;
}
// 限幅
if (ySpeed > 100)
{
ySpeed = 100;
}
if (ySpeed < -100)
{
ySpeed = -100;
}
// 控制
TH_command.AGV_MoveControl_0x70(ySpeed, 0, 0);
// 更新数据
GetUrgKB();
}
}
