/// <summary>
/// Triggeres calls to perform given action.
/// </summary>
/// <param name="action">Action that will be peformed.</param>
private void PerformAction(PanTiltHATAction action)
{
switch (action)
{
case PanTiltHATAction.EnableServo1:
isServo1Enabled = true;
WriteConfiguration();
break;
case PanTiltHATAction.EnableServo2:
isServo2Enabled = true;
WriteConfiguration();
break;
case PanTiltHATAction.DisableServo1:
isServo1Enabled = false;
WriteConfiguration();
break;
case PanTiltHATAction.DisableServo2:
isServo1Enabled = false;
WriteConfiguration();
break;
case PanTiltHATAction.Pan:
SetDegrees(PanTiltHATAction.Pan, 50);
break;
}
}
/// <summary>
/// Sets degree value to specified rotating action.
/// Will reset the servo time out timer.
/// </summary>
/// <param name="action">Executed action.</param>
/// <param name="degrees">New degree value.</param>
private void SetDegrees(PanTiltHATAction action, int degrees)
{
// Ensure valid action value.
if (action != PanTiltHATAction.Tilt && action != PanTiltHATAction.Pan)
{
throw new OperationCanceledException("Action must be .Tilt or .Pan");
}
// Stop existing time out timer.
StopServoTimeOutTimer();
// Check if tilt or pan.
var isTiltRequested = action == PanTiltHATAction.Tilt;
// Enable servo is required.
if (!(isTiltRequested ? isServo1Enabled : isServo2Enabled))
{
PerformAction(isTiltRequested ? PanTiltHATAction.EnableServo1 : PanTiltHATAction.EnableServo2);
}
// Build data bytes.
var command = isTiltRequested ? COMMANDO_SERVO_1 : COMMANDO_SERVO_2;
// Get (valid) milliseconds from degrees.
// Cancel method call if ms are not valid.
var ms = DegreesToMs(degrees);
if (ms == null)
{
return;
}
var bitmask = BitConverter.GetBytes(ms.GetValueOrDefault());
// Write to device.
WriteByte(command, bitmask);
// Start servo time out timer.
StartServoTimeOutTimer();
}
/// <summary>
/// Gets the degrees of given pan or tilt action.
/// </summary>
/// <param name="action">Defines wether pan or tilt degrees will be returned.</param>
/// <returns>Current degrees of pan or tilt action.</returns>
private int GetDegrees(PanTiltHATAction action)
{
// Ensure valid action value.
if (action != PanTiltHATAction.Tilt && action != PanTiltHATAction.Pan)
{
throw new OperationCanceledException("Action must be .Tilt or .Pan");
}
// Check if tilt or pan.
var isTiltRequested = action == PanTiltHATAction.Tilt;
// Read from device
var commando = isTiltRequested ? COMMANDO_SERVO_1 : COMMANDO_SERVO_2;
byte[] readBuffer = new byte[2];
_ = pic16f1503.WriteReadPartial(new byte[] { commando }, readBuffer);
// Convert pulses to degrees.
var degrees = MsToDegrees(readBuffer[0] | (readBuffer[1] << 8));
// Return value.
return(degrees.GetValueOrDefault());
}
