////////////////////////////////////////// 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(); } }