static void driveForward(bool slowDown = false)
{
MotorSpeed speed = slowDown ? MotorSpeed.SlowForward : MotorSpeed.NormalForward;
motorLeftSpeed = speed;
motorRightSpeed = speed;
}
public void SetForward(MotorSpeed motorSpeed)
{
requestedSpeed = motorSpeed;
var requestedSpeedDutyCycle = motorSpeedConverter.GetDutyCycleFromSpeed(motorSpeed);
motorActors.Right.SetSpeed(requestedSpeedDutyCycle);
motorActors.Left.SetSpeed(requestedSpeedDutyCycle);
}
/// <summary>
/// Picks the speed.
/// </summary>
/// <remarks>Requires behavior control; see <see cref="ControlComponent.RequestControl(int)"/>.</remarks>
/// <param name="speed">The speed.</param>
/// <param name="slow">The slow.</param>
/// <param name="medium">The medium.</param>
/// <param name="fast">The fast.</param>
/// <returns>The selected speed value</returns>
private static float PickSpeed(MotorSpeed speed, float slow, float medium, float fast)
{
switch (speed)
{
case MotorSpeed.Slow: return(slow);
case MotorSpeed.Medium: return(medium);
case MotorSpeed.Fast: return(fast);
default: throw new NotSupportedException($"Speed {speed} is not supported");
}
}
/// <summary>
/// Returns a new collection of PumpTelemetryItems from the data stored in this object.
/// </summary>
/// <returns></returns>
public IEnumerable <PumpTelemetryItem> ToPumpTelemetryItems()
{
var numItems = MotorPowerKw.Count();
var telemetryItems = new List <PumpTelemetryItem>(numItems);
for (var i = 0; i < numItems; i++)
{
telemetryItems.Add(new PumpTelemetryItem(MotorPowerKw.ElementAt(i), MotorSpeed.ElementAt(i),
PumpRate.ElementAt(i), TimePumpOn.ElementAt(i), CasingFriction.ElementAt(i)));
}
return(telemetryItems);
}
public void MotorController_SetForward_CallsMotorActorSetSpeedOnceOnEachSide(MotorSpeed motorSpeed)
{
// Arrange
var motorActors = new Mock <IDirectional <IMotorActor> >();
var motorActorLeft = new Mock <IMotorActor>();
var motorActorRight = new Mock <IMotorActor>();
motorActors.SetupGet(_ => _.Right).Returns(motorActorRight.Object);
motorActors.SetupGet(_ => _.Left).Returns(motorActorLeft.Object);
var motorSpeedConverter = new MotorSpeedConverter();
var testee = new MotorController(motorActors.Object, motorSpeedConverter);
// Act
testee.SetForward(motorSpeed);
// Assert
motorActors.Verify(p => p.Right.SetSpeed(It.IsAny <double>()), Times.Once());
motorActors.Verify(p => p.Left.SetSpeed(It.IsAny <double>()), Times.Once());
}
public double GetDutyCycleFromSpeed(MotorSpeed motorSpeed)
{
switch (motorSpeed)
{
case MotorSpeed.Slow:
return(0.2);
case MotorSpeed.Medium:
return(0.5);
case MotorSpeed.Fast:
return(0.8);
case MotorSpeed.Maximum:
return(1);
default:
throw new ArgumentOutOfRangeException(nameof(motorSpeed), motorSpeed, null);
}
}
/// <summary>
/// Save profile values
/// </summary>
public void SaveProfileSettings()
{
if (Temperature > TempMax)
{
Temperature = TempMax;
}
if (Temperature < TempMin)
{
Temperature = TempMin;
}
if (_position > MaxPosition)
{
_position = MaxPosition;
}
//ascom items
Profile.WriteValue(sCsDriverId, "FilterNames", String.Join(",", Names).ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "Absolute", Absolute.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "MaxIncrement", MaxIncrement.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "MaxPosition", MaxPosition.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "Position", _position.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "MotorSpeed", MotorSpeed.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "StepSize", stepSize.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempComp", tempComp.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempCompAvailable", TempCompAvailable.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "Temperature", Temperature.ToString(CultureInfo.InvariantCulture));
//extended FilterWheel items
Profile.WriteValue(sCsDriverId, "CanHalt", CanHalt.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "CanStepSize", CanStepSize.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "Synchronous", Synchronous.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempMax", TempMax.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempMin", TempMin.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempPeriod", TempPeriod.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempProbe", TempProbe.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "TempSteps", TempSteps.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "SettleTime", settleTime.ToString(CultureInfo.InvariantCulture));
Profile.WriteValue(sCsDriverId, "Logging", TL.Enabled.ToString(CultureInfo.InvariantCulture));
}
static void rightTurnRobot()
{
stopRobot();
motorLeftSpeed = MotorSpeed.NormalForward;
motorRightSpeed = MotorSpeed.NormalBackward;
double angle = getAngleSensor();
// turn until angle matches expected one after turn
while (
Math.Abs(angle - angleZero - (((turnCount * 90) % 360) + 90)) >
ANGLE_TOLERANCE_VALUE
)
{
angle = getAngleSensor();
// simulate turning when testing by updating angle
if (isTesting)
{
setAngleSensor(Math.Round(angle + 0.01, 2));
}
}
stopRobot();
driveForward();
// simulate turning right when testing by updating distance sensors
if (isTesting)
{
simulationTurnRight();
}
// increment turn count
turnCount++;
}
static void selfAlign()
{
double startingAngle = getAngleSensor();
bool shouldTurnLeft = startingAngle > (turnCount * 90) % 360;
if (shouldTurnLeft)
{
// turn left if needed
motorLeftSpeed = MotorSpeed.SlowForward;
motorRightSpeed = MotorSpeed.SlowBackward;
}
else
{
// turn right if needed
motorLeftSpeed = MotorSpeed.SlowBackward;
motorRightSpeed = MotorSpeed.SlowForward;
}
double angle = startingAngle;
while (
Math.Abs(angle - angleZero - ((turnCount * 90) % 360)) >
ANGLE_TOLERANCE_VALUE
)
{
angle = getAngleSensor();
if (isTesting)
{
setAngleSensor(angle + (shouldTurnLeft ? -0.005 : 0.005));
}
}
stopRobot();
driveForward();
}
public void MotorSpeedConverter_GetDutyCycleFromSpeed_ReturnsExpectedDutyCycle(MotorSpeed motorSpeed, double expectedDutyCycle)
{
// Arrange
var testee = new MotorSpeedConverter();
// Act
var result = testee.GetDutyCycleFromSpeed(motorSpeed);
// Assert
result.Should().Be(expectedDutyCycle);
}
/// <summary>
/// Moves the lift based on direction and speed
/// </summary>
/// <remarks>Requires behavior control; see <see cref="ControlComponent.RequestControl(int)"/>.</remarks>
/// <param name="moveDirection">The move direction.</param>
/// <param name="speed">The speed.</param>
/// <returns>A task that represents the asynchronous operation.</returns>
public async Task <StatusCode> MoveLift(MoveDirection moveDirection, MotorSpeed speed = MotorSpeed.Medium)
{
float liftSpeed = PickSpeed(speed, 2, 4, 8);
return(await SetLiftMotor((float)moveDirection *liftSpeed).ConfigureAwait(false));
}
/// <summary>
/// Moves the head based on direction and speed
/// </summary>
/// <remarks>Requires behavior control; see <see cref="ControlComponent.RequestControl(int)"/>.</remarks>
/// <param name="moveDirection">The move 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> MoveHead(MoveDirection moveDirection, MotorSpeed speed = MotorSpeed.Medium)
{
float headSpeed = PickSpeed(speed, 0.5f, 1, 2);
return(await SetHeadMotor((float)moveDirection *headSpeed).ConfigureAwait(false));
}
/// <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 void SetCoordinatedMotors(UpdateMotorSpeed[] motors)
{
//LogInfo("TrackRoamerBrickPowerService:SetCoordinatedMotors()");
MotorSpeed motorSpeed = new MotorSpeed() { Timestamp = DateTime.Now };
// Default null which is ignored by the controller.
motorSpeed.LeftSpeed = null;
motorSpeed.RightSpeed = null;
// Combine the motor commands
foreach (UpdateMotorSpeed ms in motors)
{
if (ms.Body.LeftSpeed != null && ms.Body.LeftSpeed >= -1.0 && ms.Body.LeftSpeed <= 1.0)
motorSpeed.LeftSpeed = ms.Body.LeftSpeed;
if (ms.Body.RightSpeed != null && ms.Body.RightSpeed >= -1.0 && ms.Body.RightSpeed <= 1.0)
motorSpeed.RightSpeed = ms.Body.RightSpeed;
}
// Send a singe command to the controller:
UpdateMotorSpeed combinedRequest = new UpdateMotorSpeed(motorSpeed);
_mainPort.Post(combinedRequest);
Activate(Arbiter.Choice(combinedRequest.ResponsePort,
delegate(DefaultUpdateResponseType response)
{
// send responses back to the original motors
foreach (UpdateMotorSpeed ms in motors)
ms.ResponsePort.Post(DefaultUpdateResponseType.Instance);
},
delegate(Fault fault)
{
// send failure back to the original motors
foreach (UpdateMotorSpeed ms in motors)
ms.ResponsePort.Post(fault);
}));
}
static void stopRobot()
{
motorLeftSpeed = MotorSpeed.Stop;
motorRightSpeed = MotorSpeed.Stop;
}
private void Awake()
{
MotorSpeed = UDPSend.motorSpeed;
}
