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