private void Refresh_FormStart(object source, System.Timers.ElapsedEventArgs e)
{
testRefresh++;
this.BeginInvoke((EventHandler) delegate
{
this.Text = testRefresh.ToString();
string[] MSG;
// 控制口数据
Class.TH_SendCommand.TH_data.IsGetting = true;
while (Class.TH_SendCommand.TH_data.IsSetting)
{
;
}
MSG = new string[10];
MSG[0] = "";
MSG[1] = "";
MSG[2] = "Head_L_X: " + Class.TH_SendCommand.TH_data.Head_L_X.ToString();
MSG[3] = "Head_L_Y: " + Class.TH_SendCommand.TH_data.Head_L_Y.ToString();
MSG[4] = "Head_R_X: " + Class.TH_SendCommand.TH_data.Head_R_X.ToString();
MSG[5] = "Head_R_Y: " + Class.TH_SendCommand.TH_data.Head_R_Y.ToString();
MSG[6] = "Tail_L_X: " + Class.TH_SendCommand.TH_data.Tail_L_X.ToString();
MSG[7] = "Tail_L_Y: " + Class.TH_SendCommand.TH_data.Tail_L_Y.ToString();
MSG[8] = "Tail_R_X: " + Class.TH_SendCommand.TH_data.Tail_R_X.ToString();
MSG[9] = "Tail_R_Y: " + Class.TH_SendCommand.TH_data.Tail_R_Y.ToString();
Class.TH_SendCommand.TH_data.IsGetting = false;
if (TH_command.IsClose)
{
MSG[0] = "CON port not Open";
}
this.controlportMSG.Lines = MSG;
// 激光雷达数据
MSG = new string[6];
MSG[0] = "";
MSG[1] = "";
// 编码器数据
// 运行时间
if (MoveTime >= 0)
{
MoveTime--;
}
if (MoveTime == 0)
{
TH_command.AGV_MoveControl_0x70(0, 0, 0);
}
});
}
////////////////////////////////////////// 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);
}
}
}
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();
}
}
