/// <summary>
/// Attempts to set the target (width of pulses sent) for a single channel.
/// </summary>
/// <param name="channel">Channel number from 0 to 23.</param>
/// <param name="us">Desired servo position, microseconds
/// </param>
private void ServoPositionSetUs(Byte channel, double us)
{
try
{
// Pololu servo target is in units of quarter microseconds.
// For typical servos, 6000 is neutral and the acceptable range is 4000-8000.
UInt16 target = (UInt16)Math.Round(us * 4.0d);
pololumaestro.ChannelValuePair cvp = new pololumaestro.ChannelValuePair()
{
Channel = channel, Target = target
};
List <pololumaestro.ChannelValuePair> channelValues = new List <pololumaestro.ChannelValuePair>();
channelValues.Add(cvp);
pololumaestro.PololuMaestroCommand cmd = new pololumaestro.PololuMaestroCommand()
{
Command = "set", ChannelValues = channelValues
};
_pololuMaestroPort.Post(new pololumaestro.SendPololuMaestroCommand(cmd));
}
catch (Exception exception)
{
LogError(exception);
}
}
// Iterator to execute the Behavior
// It is important to use an Iterator so that it can relinquish control
// when there is nothing to do, i.e. yield return
protected IEnumerator <ITask> BehaviorExercisePololuMaestro()
{
LogInfo("DriveBehaviorServiceBase: BehaviorExercisePololuMaestro() Started");
Talker.Say(10, "waiting for Behavior Exercise Pololu Maestro");
// Wait for the robot to initialize, otherwise it will miss the initial command
for (int i = 10; i > 0; i--)
{
LogInfo(LogGroups.Console, i.ToString());
yield return(Timeout(1000));
}
Talker.Say(10, "starting Behavior Exercise Pololu Maestro");
// Wait for settling time
yield return(Timeout(settlingTime));
byte channel = ServoChannelMap.leftGunTilt;
for (int i = 1; i <= 50; i++)
{
int servoPos = 1000 + 20 * i;
//Talker.Say(10, "servo " + servoPos);
pololumaestro.ChannelValuePair cvp = new pololumaestro.ChannelValuePair()
{
Channel = channel, Target = (ushort)(servoPos * 4)
};
List <pololumaestro.ChannelValuePair> channelValues = new List <pololumaestro.ChannelValuePair>();
channelValues.Add(cvp);
pololumaestro.PololuMaestroCommand cmd = new pololumaestro.PololuMaestroCommand()
{
Command = "set", ChannelValues = channelValues
};
_pololuMaestroPort.Post(new pololumaestro.SendPololuMaestroCommand(cmd));
// wait some time
yield return(Timeout(1000));
}
Talker.Say(10, "Behavior Exercise Pololu Maestro finished");
// done
yield break;
}
/// <summary>
/// Attempts to set the target (width of pulses sent) for multiple channels.
/// </summary>
/// <param name="channelValues"></param>
private void LightsSet(List <pololumaestro.ChannelValuePair> channelValues)
{
try
{
pololumaestro.PololuMaestroCommand cmd = new pololumaestro.PololuMaestroCommand()
{
Command = "set", ChannelValues = channelValues
};
_pololuMaestroPort.Post(new pololumaestro.SendPololuMaestroCommand(cmd));
}
catch (Exception exception)
{
LogError(exception);
}
}
/// <summary>
/// Attempts to set the target (width of pulses sent) for multiple channels.
/// </summary>
/// <param name="channelValues"></param>
private void ServoPositionSetUs(List<pololumaestro.ChannelValuePair> channelValues)
{
if (channelValues.Any())
{
try
{
pololumaestro.PololuMaestroCommand cmd = new pololumaestro.PololuMaestroCommand() { Command = "set", ChannelValues = channelValues };
_pololuMaestroPort.Post(new pololumaestro.SendPololuMaestroCommand(cmd));
}
catch (Exception exception)
{
LogError(exception);
}
}
}
/// <summary>
/// Attempts to set the target (width of pulses sent) for a single channel.
/// </summary>
/// <param name="channel">Channel number from 0 to 23.</param>
/// <param name="us">Desired servo position, microseconds
/// </param>
private void ServoPositionSetUs(Byte channel, double us)
{
try
{
// Pololu servo target is in units of quarter microseconds.
// For typical servos, 6000 is neutral and the acceptable range is 4000-8000.
UInt16 target = (UInt16)Math.Round(us * 4.0d);
pololumaestro.ChannelValuePair cvp = new pololumaestro.ChannelValuePair() { Channel = channel, Target = target };
List<pololumaestro.ChannelValuePair> channelValues = new List<pololumaestro.ChannelValuePair>();
channelValues.Add(cvp);
pololumaestro.PololuMaestroCommand cmd = new pololumaestro.PololuMaestroCommand() { Command = "set", ChannelValues = channelValues };
_pololuMaestroPort.Post(new pololumaestro.SendPololuMaestroCommand(cmd));
}
catch (Exception exception)
{
LogError(exception);
}
}
