// Rotate one step counter-clockwise
public static DriveDirection Next(this DriveDirection dd)
{
// There might be a mathematical solution. But in fewer instructions than a branch miss?
switch (dd)
{
case DriveDirection.East: return(DriveDirection.NorthEast);
case DriveDirection.NorthEast: return(DriveDirection.North);
case DriveDirection.North: return(DriveDirection.NorthWest);
case DriveDirection.NorthWest: return(DriveDirection.West);
case DriveDirection.West: return(DriveDirection.SouthWest);
case DriveDirection.SouthWest: return(DriveDirection.South);
case DriveDirection.South: return(DriveDirection.SouthEast);
case DriveDirection.SouthEast: return(DriveDirection.East);
default:
return(0);
}
}
/// <summary>
/// Drives Vector based on direction and speed
/// </summary>
/// <remarks>Requires behavior control; see <see cref="ControlComponent.RequestControl(int)"/>.</remarks>
/// <param name="driveDirection">The drive direction.</param>
/// <param name="turnDirection">The turn direction.</param>
/// <param name="speed">The speed.</param>
/// <returns>A task that represents the asynchronous operation; the task result contains the result from the robot.</returns>
public async Task <StatusCode> Drive(DriveDirection driveDirection, TurnDirection turnDirection, MotorSpeed speed = MotorSpeed.Medium)
{
float driveSpeed = PickSpeed(speed, 50, 75, 150);
float turnSpeed = PickSpeed(speed, 30, 50, 100);
float driveValue = (float)driveDirection;
float turnValue = (float)turnDirection;
if (driveValue < 0)
{
turnValue = -turnValue;
}
float leftWheelSpeed = (driveValue * driveSpeed) + (turnValue * turnSpeed);
float rightWheelSpeed = (driveValue * driveSpeed) - (turnValue * turnSpeed);
return(await SetWheelMotors(leftWheelSpeed, rightWheelSpeed, leftWheelSpeed * 4, rightWheelSpeed * 4).ConfigureAwait(false));
}
public static DriveDirection GetDriveDirection(int x, int z, DriveDirection previous,
NavRegionProjection projection)
{
// This incredible switch construct is so that we can jump directly to the previous value, saving us a lot of code in the general case
// And, in particular, allowing us to avoid shuffling our large arguments between stacks, as the dopey JIT won't inline the large comparisons used here
// The use of non-short-circuiting is a speculative optimisation against expected cost of branch mis-prediction (NOTE: inspected JIT but haven't profiled yet -AR)
// This slightly expands our output code size (~5 icache lines total), but perhaps not a big deal due to the switch behaviour in the typical case.
switch (previous)
{
default:
previous = 0; // Reset
goto case DriveDirection.East;
case DriveDirection.East:
{
if (((x < projection.x.min) & (projection.z.min <= z) & (z <= projection.z.max)) |
((x < projection.negative.min) & (x < projection.positive.min)))
{
return(DriveDirection.East);
}
}
if (previous == 0)
{
goto case DriveDirection.West;
}
else
{
goto default;
}
case DriveDirection.West:
{
if (((x > projection.x.max) & (projection.z.min <= z) & (z <= projection.z.max)) |
((x > projection.negative.max) & (x > projection.positive.max)))
{
return(DriveDirection.West);
}
}
if (previous == 0)
{
goto case DriveDirection.North;
}
else
{
goto default;
}
case DriveDirection.North:
{
if (((z < projection.z.min) & (projection.x.min <= x) & (x <= projection.x.max)) |
((x < projection.negative.min) & (x > projection.positive.max)))
{
return(DriveDirection.North);
}
}
if (previous == 0)
{
goto case DriveDirection.South;
}
else
{
goto default;
}
case DriveDirection.South:
{
if (((z > projection.z.max) & (projection.x.min <= x) & (x <= projection.x.max)) |
((x > projection.negative.max) & (x < projection.positive.min)))
{
return(DriveDirection.South);
}
}
if (previous == 0)
{
goto case DriveDirection.NorthEast;
}
else
{
goto default;
}
case DriveDirection.NorthEast:
{
if (((x <= projection.x.max) & (z <= projection.z.max) & (projection.positive.min <= x) &
(x <= projection.positive.max))
| ((x < projection.x.min) & (z < projection.z.min)))
{
return(DriveDirection.NorthEast);
}
}
if (previous == 0)
{
goto case DriveDirection.NorthWest;
}
else
{
goto default;
}
case DriveDirection.NorthWest:
{
if (((x >= projection.x.min) & (z <= projection.z.max) & (projection.negative.min <= x) &
(x <= projection.negative.max))
| ((x > projection.x.max) & (z < projection.z.min)))
{
return(DriveDirection.NorthWest);
}
}
if (previous == 0)
{
goto case DriveDirection.SouthEast;
}
else
{
goto default;
}
case DriveDirection.SouthEast:
{
if (((x <= projection.x.max) & (z >= projection.z.min) & (projection.negative.min <= x) &
(x <= projection.negative.max))
| ((x < projection.x.min) & (z > projection.z.max)))
{
return(DriveDirection.SouthEast);
}
}
if (previous == 0)
{
goto case DriveDirection.SouthWest;
}
else
{
goto default;
}
case DriveDirection.SouthWest:
{
if (((x >= projection.x.min) & (z >= projection.z.min) & (projection.positive.min <= x) &
(x <= projection.positive.max))
| ((x > projection.x.max) & (z > projection.z.max)))
{
return(DriveDirection.SouthWest);
}
}
if (previous == 0)
{
return(0); // Nowhere to go :(
}
else
{
goto default;
}
}
}
public static int GetZ(this DriveDirection dd)
{
return((int)((uint)dd << 28) >> 30); // <- Fill sign bit
}
public DirectedEdge(IP2PEdge edge, int nodeFromId)
{
Edge = edge;
Direction = nodeFromId == Edge.StartNode.Id ? DriveDirection.Forward : DriveDirection.Backward;
}
public DirectedEdge(IP2PEdge edge, DriveDirection direction = DriveDirection.Forward)
{
Edge = edge;
Direction = direction;
}
/// <summary>
/// Create new instance
/// </summary>
/// <param name="Direction">Drive direction</param>
public DriverCmd(DriveDirection Direction)
{
this.Direction = Direction;
}
public override void HandleInput(IOperatorInputMsg Input)
{
var keyConfig = _keyBinding.CurrentValue.Driver;
//forward
if (ValidateKeyPress(Input, keyConfig.Forward))
{
if (Input.InputType == KeyInputType.KeyPress)
{
if (_currDirection == DriveDirection.Forward)
{
_currDirection = DriveDirection.Stop;
_cmdDelegate.Stop();
}
else
{
_currDirection = DriveDirection.Forward;
_cmdDelegate.DriveForward();
}
}
else if (Input.InputType == KeyInputType.KeyDown)
{
_currDirection = DriveDirection.Forward;
_cmdDelegate.DriveForward();
}
else
{
if (_currDirection == DriveDirection.Forward)
{
_currDirection = DriveDirection.Stop;
_cmdDelegate.Stop();
}
}
Input.IsHandled = true;
}
//back
else if (ValidateKeyPress(Input, keyConfig.Backward))
{
if (Input.InputType == KeyInputType.KeyPress)
{
if (_currDirection == DriveDirection.Backward)
{
_currDirection = DriveDirection.Stop;
_cmdDelegate.Stop();
}
else
{
_currDirection = DriveDirection.Backward;
_cmdDelegate.DriveBackward();
}
}
else if (Input.InputType == KeyInputType.KeyDown)
{
_currDirection = DriveDirection.Backward;
_cmdDelegate.DriveBackward();
}
else
{
if (_currDirection == DriveDirection.Backward)
{
_currDirection = DriveDirection.Stop;
_cmdDelegate.Stop();
}
}
Input.IsHandled = true;
}
}
