private Motor.Directions DestinationMotorDirection(ActionPoint destination, TransferCarJob.PickDropInfo.PickDropTypes type)
{
var result = Motor.Directions.Forward;
var side = TransferSide(destination);
var dir = destination.Parent.End - destination.Parent.Start;
var mydir = new Vector3(0, 0, 1);
mydir.TransformCoordinate(transferCar.Orientation);
var dot = Vector3.Dot(dir, mydir);
if (side == TransferCar.TransferCarSide.Left)
{
if (dot < 0)
{
result = type == TransferCarJob.PickDropInfo.PickDropTypes.Pick ? Motor.Directions.Forward : Motor.Directions.Backward;
}
else
{
result = type == TransferCarJob.PickDropInfo.PickDropTypes.Pick ? Motor.Directions.Backward : Motor.Directions.Forward;
}
}
if (side == TransferCar.TransferCarSide.Right)
{
if (dot > 0)
{
result = type == TransferCarJob.PickDropInfo.PickDropTypes.Drop ? Motor.Directions.Backward : Motor.Directions.Forward;
}
else
{
result = type == TransferCarJob.PickDropInfo.PickDropTypes.Drop ? Motor.Directions.Forward : Motor.Directions.Backward;
}
}
return(result);
}
private void SetTransportMotorAndRoute(StraightTransportSection carTransport, Motor.Directions direction, Route next, TransferCarJob.PickDropInfo.PickDropTypes jobType, ActionPoint leaving)
{
if (direction == Motor.Directions.Forward)
{
if (carTransport.Route.Motor.Direction != direction)
{
carTransport.Route.Motor.SwitchDirection();
}
if (next.Motor.Direction != direction)
{
next.Motor.SwitchDirection();
}
if (jobType == TransferCarJob.PickDropInfo.PickDropTypes.Drop)
{
carTransport.Route.NextRoute = next;
}
else
{
next.NextRoute = carTransport.Route;
}
leaving.Distance = carTransport.Length;
carTransport.Route.Motor.Start();
}
else
{
if (carTransport.Route.Motor.Direction != direction)
{
carTransport.Route.Motor.SwitchDirection();
}
if (next.Motor.Direction != direction)
{
next.Motor.SwitchDirection();
}
if (jobType == TransferCarJob.PickDropInfo.PickDropTypes.Drop)
{
carTransport.Route.PreviousRoute = next;
}
else
{
next.PreviousRoute = carTransport.Route;
}
leaving.Distance = 0;
carTransport.Route.Motor.Start();
}
}
private void SetTransportLocalYaw(StraightTransportSection carTransport, TransferCarSide side, Motor.Directions direction, TransferCarJob.PickDropInfo.PickDropTypes jobType)
{
if (jobType == TransferCarJob.PickDropInfo.PickDropTypes.Pick)
{
if (direction == Motor.Directions.Forward)
{
if (side == TransferCarSide.Right)
{
carTransport.LocalYaw = (float)Math.PI / 2;
}
else
{
carTransport.LocalYaw = -(float)Math.PI / 2;
}
}
else
{
if (side == TransferCarSide.Right)
{
carTransport.LocalYaw = -(float)Math.PI / 2;
}
else
{
carTransport.LocalYaw = (float)Math.PI / 2;
}
}
}
else
{
if (direction == Motor.Directions.Forward)
{
if (side == TransferCarSide.Left)
{
carTransport.LocalYaw = (float)Math.PI / 2;
}
else
{
carTransport.LocalYaw = -(float)Math.PI / 2;
}
}
else
{
if (side == TransferCarSide.Left)
{
carTransport.LocalYaw = -(float)Math.PI / 2;
}
else
{
carTransport.LocalYaw = (float)Math.PI / 2;
}
}
}
}