private void DoSetHeading(int newHeadingDegrees)
{
var headingChange = newHeadingDegrees - _currentHeading;
while (headingChange >= 360)
{
headingChange -= 360;
}
while (headingChange < 0)
{
headingChange += 360;
}
if (headingChange < 15.0 || headingChange > 345.0)
{
return;
}
_currentHeading = newHeadingDegrees;
SpheroTrace.Trace("Heading to {0})", headingChange);
var command = new SetHeadingCommand(headingChange);
SendCommand(command);
}
private void SendCommand(IConnectedSphero sphero, ISpheroCommand command)
{
if (sphero == null)
{
// TODO - trace this ignore
SpheroTrace.Trace("Ignoring command {0}", command);
return;
}
//SpheroTrace.Trace("Sending command {0}", command);
sphero.SendAndReceive(command, OnCommandSuccess, OnCommandError);
}
protected void SendCommand(ISpheroCommand command)
{
var sphero = Parent.ConnectedSphero;
if (sphero == null)
{
// TODO - trace this ignore
SpheroTrace.Trace("Ignoring command {0}", command);
return;
}
SpheroTrace.Trace("Sending command {0}", command);
sphero.SendAndReceive(command, OnCommandSuccess, OnCommandError);
}
protected void DoRoll(CartesianPositionParameters positionParameters)
{
// TODO - put the check back in for 'too close'
//if (Position.AbsoluteDistance < DistanceThreshold)
//{
// return;
//}
Position = positionParameters;
int headingDegrees = positionParameters.HeadingDegrees;
var speed = (int)((SpeedService.SpeedPercent * positionParameters.AbsoluteDistance) / 100.0);
SpheroTrace.Trace("Rolling to {0} with speed {1})", headingDegrees, speed);
var rollCommand = new RollCommand(speed, headingDegrees, false);
SendCommand(rollCommand);
}