/// <summary>
///
/// </summary>
/// <param name="wheelOutputs"></param>
public void Play(OutputToWheels wheelOutputs)
{
// deduce values of power
var leftMotPower = ExtractMotorPower(AbsMotorPower, wheelOutputs.LeftMotorPolarity);
var rightMotPower = ExtractMotorPower(AbsMotorPower, wheelOutputs.RightMotorPolarity);
// sleep if delay
if (wheelOutputs.DelayInSeconds > 0)
{
System.Threading.Thread.Sleep(wheelOutputs.DelayInSeconds * 1000);
}
// MOVE!
controller.MoveLeftWheel(leftMotPower, wheelOutputs.LeftMotorTachocount);
controller.MoveRightWheel(rightMotPower, wheelOutputs.RightMotorTachocount);
}
/// <summary>
///
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="inhibitPhysicalMovement"></param>
/// <returns></returns>
public OutputToWheels Drive(int x, int y, bool inhibitPhysicalMovement)
{
// outputs
OutputToWheels output = new OutputToWheels();
// polar radius from cartesian coordinate
var radius = Math.Sqrt(x * x + y * y);
// angle (radians) from cartesian coordinate
var angle_radians = Math.Acos(Math.Abs(x) / radius);
if (double.IsNaN(angle_radians))
{
angle_radians = 0;
}
// angle (degrees) from cartesian coordinate
var angle_degrees = angle_radians * 180 / Math.PI;
// turn coeffecient i.e. scale of deviation from 45-degrees (NE, NW, SE, SW)
var tcoeff = (angle_degrees / 45) - 1; //-1 + (angle_degrees/ 90) * 2;
// raw output of the turn motor
var rawTurn = tcoeff * Math.Abs((Math.Abs(y) - Math.Abs(x)));
rawTurn = Math.Round(rawTurn * 100) / 100;
var turnMotPolarity = ExtractMotorPolarity(Convert.ToSingle(rawTurn));
var turnMotTacho = Convert.ToUInt32((Math.Abs(rawTurn) * _distancePerDegreeInCM));
// raw output of the distance motor
var rawDistance = Math.Max(Math.Abs(y), Math.Abs(x));
var distanceMotPolarity = ExtractMotorPolarity(rawDistance);
var distanceMotTacho = Convert.ToUInt32((Math.Abs(rawDistance) * _distancePerDegreeInCM));
//// re-map values to correct output range (from -G to +G)
//var valLeft = ReMap(rawLeft, _minXYVal, _maxXYVal, minOutputVal, maxOutputVal);
//var valRight = ReMap(rawRight, _minXYVal, _maxXYVal, minOutputVal, maxOutputVal);
// if input coordinate in 1st or 3rd quadrant; left motor is Distance, right motor is Turn
if ((x >= 0 && y >= 0) || (x < 0 && y < 0))
{
output.LeftMotorPolarity = distanceMotPolarity;
output.LeftMotorTachocount = distanceMotTacho;
output.RightMotorPolarity = turnMotPolarity;
output.RightMotorTachocount = turnMotTacho;
}
else
{
output.RightMotorPolarity = distanceMotPolarity;
output.RightMotorTachocount = distanceMotTacho;
output.LeftMotorPolarity = turnMotPolarity;
output.LeftMotorTachocount = turnMotTacho;
}
// Reverse polarity
//if (y < 0)
//{
// rawLeft = (float)(0.0 - rawLeft);
// rawRight = (float)(0.0 - rawRight);
//}
// play generated output (MOVE!)
if (!inhibitPhysicalMovement)
{
Play(output);
}
// return generated outputs.
return(output);
}
