public void UpdateDeliverData()
{
rtVector PredictPosition = new rtVector();
rtAGV_Data src = new rtAGV_Data();
rtAGV_Data dst = new rtAGV_Data();
//讀取PLC的高度及速度資訊
if (DeliverData.tAGV_Data.ucAGV_Status != 0) GetFrontWheelSpeed();
//紀錄讀取完的時間
//GlobalVar.Watch_Read_PLC_Data.Start();
//GlobalVar.Time_Read_PLC_Data = DateTime.Now;
//計算預測座標
double PredictAngle = 0;
DiffTimeForAlignment(ref PredictPosition, ref PredictAngle, DeliverData.tAGV_Data.CMotor);
src.tCarInfo.tPosition.eX = PredictPosition.eX;
src.tCarInfo.tPosition.eY = PredictPosition.eY;
src.tCarInfo.eAngle = PredictAngle;
//定位座標轉換為車體座標資訊
CoordinateTransformProcess(src, ref dst);
//更新車體資訊
DeliverData.tAGV_Data.tCarInfo.tPosition.eX = PredictPosition.eX;
DeliverData.tAGV_Data.tCarInfo.tPosition.eY = PredictPosition.eY;
DeliverData.tAGV_Data.tCarInfo.eAngle = PredictAngle;
DeliverData.tAGV_Data.tCarInfo.eCarTireSpeedLeft = GlobalVar.CarTireSpeedLeft; ;
DeliverData.tAGV_Data.tCarInfo.eCarTireSpeedRight = GlobalVar.CarTireSpeedRight;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionR.eX = dst.tCarInfo.tCarTirepositionR.eX;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionR.eY = dst.tCarInfo.tCarTirepositionR.eY;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionL.eX = dst.tCarInfo.tCarTirepositionL.eX;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionL.eY = dst.tCarInfo.tCarTirepositionL.eY;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eX = dst.tCarInfo.tMotorPosition.eX;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eY = dst.tCarInfo.tMotorPosition.eY;
/*DeliverData.tAGV_Data.tCarInfo.tPosition.eX = 17050;
DeliverData.tAGV_Data.tCarInfo.tPosition.eY = -950;
DeliverData.tAGV_Data.tCarInfo.eAngle = 90;
/*DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eX = 17223;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eY = -3058;*/
//更新車Sensor資訊
DeliverData.tAGV_Data.tCarInfo.eWheelAngle = GlobalVar.RealMotorAngle;
DeliverData.tAGV_Data.tSensorData.tForkInputData.height = (int)GlobalVar.ForkCurrentHeight;
/* DeliverData.tAGV_Data.tSensorData.tForkInputData.height = (int)120;
DeliverData.tAGV_Data.ucAGV_Status = (byte)7;*/
}
private void CoordinateTransformProcess(rtAGV_Data src, ref rtAGV_Data dst)
{
//定位座標轉換為車體座標資訊
dst.tCarInfo.tPosition.eX = src.tCarInfo.tPosition.eX;
dst.tCarInfo.tPosition.eY = src.tCarInfo.tPosition.eY;
dst.tCarInfo.eAngle = src.tCarInfo.eAngle;
rtVector L_wheelPosition;
rtVector R_wheelPosition;
rtVector Motor_Position;
L_wheelPosition.eX = src.tCarInfo.tPosition.eX;
L_wheelPosition.eY = src.tCarInfo.tPosition.eY + 600;
R_wheelPosition.eX = src.tCarInfo.tPosition.eX;
R_wheelPosition.eY = src.tCarInfo.tPosition.eY - 600;
Motor_Position.eX = src.tCarInfo.tPosition.eX - 1500;
Motor_Position.eY = src.tCarInfo.tPosition.eY;
Trasform_rtVector(src.tCarInfo.tPosition, L_wheelPosition, ref dst.tCarInfo.tCarTirepositionL, src.tCarInfo.eAngle);
Trasform_rtVector(src.tCarInfo.tPosition, R_wheelPosition, ref dst.tCarInfo.tCarTirepositionR, src.tCarInfo.eAngle);
Trasform_rtVector(src.tCarInfo.tPosition, Motor_Position, ref dst.tCarInfo.tMotorPosition, src.tCarInfo.eAngle);
}
