// Sets the target position of the specified servo, causing this servo to begin moving
// to the new position. This target position is first bounded within the servo's min/max
// position limits, and a warning is logged if a position outside of these limits was
// specified.
private void SetServoPosition(Servo servo, double position)
{
if (position < servo.positionLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to minimum limit " + servo.positionLimitMin.ToString());
// Bound to this limit.
position = servo.positionLimitMin;
}
if (position > servo.positionLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " position " + position.ToString() + " bound to maximum limit " + servo.positionLimitMax.ToString());
// Bound to this limit.
position = servo.positionLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " position to " + position.ToString());
try
{
// Send this value to the hardware.
// Note that the servo position values are handled in this class in units of μs,
// to match the convention used by Pololu's Maestro Control Center application.
// However, the servo controller hardware expects the position represented as an
// integer value in 0.25 μs. The local value must be multiplied by 4 to convert
// to these units.
uscDevice.setTarget((byte)servo.index, (ushort)(position * 4));
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoPosition(): " + ex.Message);
}
}
private void TrySetTarget(List <ChannelValuePair> channelValues)
{
try
{
var groupedByDevice = from a in channelValues
group a by a.Channel / CHANNELS_PER_DEVICE into g
select new { deviceIndex = g.Key, deviceChannelValues = g };
foreach (var devGrp in groupedByDevice)
{
using (Usc device = connectToDevice(devGrp.deviceIndex)) // Find a device and temporarily connect.
{
foreach (ChannelValuePair cvp in devGrp.deviceChannelValues)
{
byte channel = (byte)(cvp.Channel % CHANNELS_PER_DEVICE);
//Console.WriteLine(" (m) device: {0} channel: {1} target: {2}", device.getSerialNumber(), channel, cvp.Target);
device.setTarget(channel, cvp.Target);
}
// device.Dispose() is called automatically when the "using" block ends,
// allowing other functions and processes to use the device.
}
}
}
catch (Exception exception) // Handle exceptions by displaying them to the user.
{
Console.WriteLine(exception);
}
}
/// <summary>
/// Steps through a simple sequence, sending servo 0 to 4000, 6000, then 8000, with 1 s between frames.
/// </summary>
private void RunSequence()
{
if (sequence_counter < 10)
{
usc.setTarget(0, 4000);
}
else if (sequence_counter < 20)
{
usc.setTarget(0, 6000);
}
else if (sequence_counter < 30)
{
usc.setTarget(0, 8000);
}
else
{
sequence_counter = 0;
}
// increment the counter by 1 every 100 ms
sequence_counter += 1;
}
/// <summary>
/// Attempts to set the target (width of pulses sent) of a channel.
/// </summary>
/// <param name="channel">Channel number from 0 to 23.</param>
/// <param name="target">
/// Target, in units of quarter microseconds. For typical servos,
/// 6000 is neutral and the acceptable range is 4000-8000.
/// </param>
void TrySetTarget(Byte channel, UInt16 target)
{
try
{
using (Usc device = connectToDevice()) // Find a device and temporarily connect.
{
device.setTarget(channel, target);
// device.Dispose() is called automatically when the "using" block ends,
// allowing other functions and processes to use the device.
}
}
catch (Exception exception) // Handle exceptions by displaying them to the user.
{
displayException(exception);
}
}
/// <summary>
/// Attempts to set the target (width of pulses sent) of a channel.
/// </summary>
/// <param name="channel">Channel number from 0 to 23.</param>
/// <param name="target">
/// Target, in units of quarter microseconds. For typical servos,
/// 6000 is neutral and the acceptable range is 4000-8000.
/// </param>
void TrySetTarget(Byte channel, int target)
{
try
{
// using () // Find a device and temporarily connect.
{
try{
//Console.WriteLine(this.Name+" Target :"+target*4);
if ((target * 4) < UInt16.MaxValue)
{
//Console.WriteLine("Servo:"+channel+" target:"+target)
channels[channel] = Convert.ToUInt16(target * 4);
//usbdevice.SetAllChannels(channels);
if (ImmediateMode)
{
if (usbdevice == null)
{
connectToDevice();
}
Console.WriteLine("immediate Set");
usbdevice.setTarget(channel, Convert.ToUInt16(target * 4));
}
}
else
{
Console.WriteLine("Invalid Target: " + target + " for Channel: " + this.channel);
}
}catch (Exception ex) {
Console.WriteLine("Invalid Target " + target + ex.Message + ex.StackTrace.ToString());
usbdevice.disconnect();
usbdevice = null;
connectToDevice();
}
// Console.WriteLine("Servo: "+channel+" mit wert "+target);
// device.Dispose() is called automatically when the "using" block ends,
// allowing other functions and processes to use the device.
}
}
catch (Exception exception) // Handle exceptions by displaying them to the user.
{
Console.WriteLine(exception.Message + "\n" + exception.StackTrace.ToString());
// displayException(exception);
}
}
/// <summary>
/// Attempts to set the target (width of pulses sent) of a channel.
/// </summary>
/// <param name="channel">Channel number - from 0 to 23.</param>
/// <param name="target">
/// Target, in units of quarter microseconds. For typical servos,
/// 6000 is neutral and the acceptable range is 4000-8000.
/// A good servo will take 880 to 2200 us (3520 to 8800 in quarters)
/// </param>
private void TrySetTarget(Byte channel, UInt16 target)
{
try
{
int index = channel / CHANNELS_PER_DEVICE;
channel = (byte)(channel % CHANNELS_PER_DEVICE);
using (Usc device = connectToDevice(index)) // Find a device and temporarily connect.
{
//Console.WriteLine(" (s) device: {0} channel: {1} target: {2}", device.getSerialNumber(), channel, target);
device.setTarget(channel, target);
// device.Dispose() is called automatically when the "using" block ends,
// allowing other functions and processes to use the device.
}
}
catch (Exception exception) // Handle exceptions by displaying them to the user.
{
Console.WriteLine(exception);
}
}
static void Main(string[] args)
{
CommandOptions opts = new CommandOptions(Assembly.GetExecutingAssembly().GetName()+"\n"+
"Select one of the following actions:\n"+
" --list list available devices\n"+
" --configure FILE load configuration file into device\n"+
" --getconf FILE read device settings and write configuration file\n"+
" --restoredefaults restore factory settings\n"+
" --program FILE compile and load bytecode program\n"+
" --status display complete device status\n"+
" --bootloader put device into bootloader (firmware upgrade) mode\n"+
" --stop stops the script running on the device\n"+
" --start starts the script running on the device\n"+
" --restart restarts the script at the beginning\n"+
" --step runs a single instruction of the script\n"+
" --sub NUM calls subroutine n (can be hex or decimal)\n"+
" --sub NUM,PARAMETER calls subroutine n with a parameter (hex or decimal)\n"+
" placed on the stack\n"+
" --servo NUM,TARGET sets the target of servo NUM in units of\n" +
" 1/4 microsecond\n"+
" --speed NUM,SPEED sets the speed limit of servo NUM\n"+
" --accel NUM,ACCEL sets the acceleration of servo NUM to a value 0-255\n"+
"Select which device to perform the action on (optional):\n"+
" --device 00001430 (optional) select device #00001430\n",
args);
if (opts["list"] != null)
{
if (opts.Count > 1)
opts.error();
listDevices();
return;
}
if (opts.Count == 0)
opts.error();
// otherwise, they must connect to a device
List<DeviceListItem> list = Usc.getConnectedDevices();
if (list.Count == 0)
{
System.Console.WriteLine("No " + Usc.englishName + " devices found.");
return;
}
DeviceListItem item = null;
// see if the device they specified was in the list
if (opts["device"] == null)
{
// Conenct to the first item in the list.
item = list[0];
}
else
{
// Remove the leading # sign. It is not standard to put it there,
// but if someone writes it, the program should still work.
string check_serial_number = opts["device"].TrimStart('#');
// Find the device with the specified serial number.
foreach (DeviceListItem check_item in list)
{
if (check_item.serialNumber == check_serial_number)
{
item = check_item;
break;
}
}
if (item == null)
{
Console.WriteLine("Could not find a " + Usc.englishName + " device with serial number " + opts["device"] + ".");
Console.WriteLine("To list devices, use the --list option.");
return;
}
}
Usc usc = new Usc(item);
if (opts["bootloader"] != null)
{
if (opts.Count > 2)
opts.error();
usc.startBootloader();
return;
}
else if (opts["status"] != null)
{
if (opts["status"] != "")
opts.error();
displayStatus(usc);
}
else if (opts["getconf"] != null)
{
getConf(usc, opts["getconf"]);
}
else if (opts["configure"] != null)
{
configure(usc, opts["configure"]);
}
else if (opts["restoredefaults"] != null)
{
if (opts["restoredefaults"] != "")
opts.error();
restoreDefaultConfiguration(usc);
}
else if (opts["program"] != null)
{
program(usc, opts["program"]);
}
else if (opts["stop"] != null)
{
setScriptDone(usc, 1);
}
else if (opts["start"] != null)
{
setScriptDone(usc, 0);
}
else if (opts["restart"] != null)
{
System.Console.Write("Restarting script...");
usc.restartScript();
usc.setScriptDone(0);
System.Console.WriteLine("");
}
else if (opts["step"] != null)
{
setScriptDone(usc, 2);
}
else if (opts["servo"] != null)
{
string[] parts = opts["servo"].Split(',');
if(parts.Length != 2)
opts.error("Wrong number of commas in the argument to servo.");
byte servo=0;
ushort target=0;
try
{
servo = byte.Parse(parts[0]);
target = ushort.Parse(parts[1]);
}
catch(FormatException)
{
opts.error();
}
Console.Write("Setting target of servo "+servo+" to "+target+"...");
usc.setTarget(servo, target);
Console.WriteLine("");
}
else if (opts["speed"] != null)
{
string[] parts = opts["speed"].Split(',');
if(parts.Length != 2)
opts.error("Wrong number of commas in the argument to speed.");
byte servo=0;
ushort speed=0;
try
{
servo = byte.Parse(parts[0]);
speed = ushort.Parse(parts[1]);
}
catch(FormatException)
{
opts.error();
}
Console.Write("Setting speed of servo "+servo+" to "+speed+"...");
usc.setSpeed(servo, speed);
Console.WriteLine("");
}
else if (opts["accel"] != null)
{
string[] parts = opts["accel"].Split(',');
if(parts.Length != 2)
opts.error("Wrong number of commas in the argument to accel.");
byte servo=0;
byte acceleration=0;
try
{
servo = byte.Parse(parts[0]);
acceleration = byte.Parse(parts[1]);
}
catch(FormatException)
{
opts.error();
}
Console.Write("Setting acceleration of servo "+servo+" to "+acceleration+"...");
usc.setAcceleration(servo, acceleration);
Console.WriteLine("");
}
else if (opts["sub"] != null)
{
string[] parts = opts["sub"].Split(new char[] {','});
if(parts.Length > 2)
opts.error("Too many commas in the argument to sub.");
byte address=0;
short parameter=0;
try
{
if(parts[0].StartsWith("0x"))
address = byte.Parse(parts[0].Substring(2),System.Globalization.NumberStyles.AllowHexSpecifier);
else
address = byte.Parse(parts[0]);
}
catch(FormatException)
{
opts.error();
}
if(parts.Length == 2)
{
try
{
if(parts[1].StartsWith("0x"))
parameter = short.Parse(parts[1].Substring(2),System.Globalization.NumberStyles.AllowHexSpecifier);
else
parameter = short.Parse(parts[1]);
}
catch(FormatException)
{
opts.error();
}
Console.Write("Restarting at subroutine "+address+" with parameter "+parameter+"...");
usc.restartScriptAtSubroutineWithParameter(address, parameter);
usc.setScriptDone(0);
}
else
{
Console.Write("Restarting at subroutine "+address+"...");
usc.restartScriptAtSubroutine(address);
usc.setScriptDone(0);
}
Console.WriteLine("");
}
else opts.error();
}
static void Main(string[] args)
{
CommandOptions opts = new CommandOptions(Assembly.GetExecutingAssembly().GetName() + "\n" +
"Select one of the following actions:\n" +
" --list list available devices\n" +
" --configure FILE load configuration file into device\n" +
" --getconf FILE read device settings and write configuration file\n" +
" --restoredefaults restore factory settings\n" +
" --program FILE compile and load bytecode program\n" +
" --status display complete device status\n" +
" --bootloader put device into bootloader (firmware upgrade) mode\n" +
" --stop stops the script running on the device\n" +
" --start starts the script running on the device\n" +
" --restart restarts the script at the beginning\n" +
" --step runs a single instruction of the script\n" +
" --sub NUM calls subroutine n (can be hex or decimal)\n" +
" --sub NUM,PARAMETER calls subroutine n with a parameter (hex or decimal)\n" +
" placed on the stack\n" +
" --servo NUM,TARGET sets the target of servo NUM in units of\n" +
" 1/4 microsecond\n" +
" --speed NUM,SPEED sets the speed limit of servo NUM\n" +
" --accel NUM,ACCEL sets the acceleration of servo NUM to a value 0-255\n" +
"Select which device to perform the action on (optional):\n" +
" --device 00001430 (optional) select device #00001430\n",
args);
if (opts["list"] != null)
{
if (opts.Count > 1)
{
opts.error();
}
listDevices();
return;
}
if (opts.Count == 0)
{
opts.error();
}
// otherwise, they must connect to a device
List <DeviceListItem> list = Usc.getConnectedDevices();
if (list.Count == 0)
{
System.Console.WriteLine("No " + Usc.englishName + " devices found.");
return;
}
DeviceListItem item = null;
// see if the device they specified was in the list
if (opts["device"] == null)
{
// Conenct to the first item in the list.
item = list[0];
}
else
{
// Remove the leading # sign. It is not standard to put it there,
// but if someone writes it, the program should still work.
string check_serial_number = opts["device"].TrimStart('#');
// Find the device with the specified serial number.
foreach (DeviceListItem check_item in list)
{
if (check_item.serialNumber == check_serial_number)
{
item = check_item;
break;
}
}
if (item == null)
{
Console.WriteLine("Could not find a " + Usc.englishName + " device with serial number " + opts["device"] + ".");
Console.WriteLine("To list devices, use the --list option.");
return;
}
}
Usc usc = new Usc(item);
if (opts["bootloader"] != null)
{
if (opts.Count > 2)
{
opts.error();
}
usc.startBootloader();
return;
}
else if (opts["status"] != null)
{
if (opts["status"] != "")
{
opts.error();
}
displayStatus(usc);
}
else if (opts["getconf"] != null)
{
getConf(usc, opts["getconf"]);
}
else if (opts["configure"] != null)
{
configure(usc, opts["configure"]);
}
else if (opts["restoredefaults"] != null)
{
if (opts["restoredefaults"] != "")
{
opts.error();
}
restoreDefaultConfiguration(usc);
}
else if (opts["program"] != null)
{
program(usc, opts["program"]);
}
else if (opts["stop"] != null)
{
setScriptDone(usc, 1);
}
else if (opts["start"] != null)
{
setScriptDone(usc, 0);
}
else if (opts["restart"] != null)
{
System.Console.Write("Restarting script...");
usc.restartScript();
usc.setScriptDone(0);
System.Console.WriteLine("");
}
else if (opts["step"] != null)
{
setScriptDone(usc, 2);
}
else if (opts["servo"] != null)
{
string[] parts = opts["servo"].Split(',');
if (parts.Length != 2)
{
opts.error("Wrong number of commas in the argument to servo.");
}
byte servo = 0;
ushort target = 0;
try
{
servo = byte.Parse(parts[0]);
target = ushort.Parse(parts[1]);
}
catch (FormatException)
{
opts.error();
}
Console.Write("Setting target of servo " + servo + " to " + target + "...");
usc.setTarget(servo, target);
Console.WriteLine("");
}
else if (opts["speed"] != null)
{
string[] parts = opts["speed"].Split(',');
if (parts.Length != 2)
{
opts.error("Wrong number of commas in the argument to speed.");
}
byte servo = 0;
ushort speed = 0;
try
{
servo = byte.Parse(parts[0]);
speed = ushort.Parse(parts[1]);
}
catch (FormatException)
{
opts.error();
}
Console.Write("Setting speed of servo " + servo + " to " + speed + "...");
usc.setSpeed(servo, speed);
Console.WriteLine("");
}
else if (opts["accel"] != null)
{
string[] parts = opts["accel"].Split(',');
if (parts.Length != 2)
{
opts.error("Wrong number of commas in the argument to accel.");
}
byte servo = 0;
byte acceleration = 0;
try
{
servo = byte.Parse(parts[0]);
acceleration = byte.Parse(parts[1]);
}
catch (FormatException)
{
opts.error();
}
Console.Write("Setting acceleration of servo " + servo + " to " + acceleration + "...");
usc.setAcceleration(servo, acceleration);
Console.WriteLine("");
}
else if (opts["sub"] != null)
{
string[] parts = opts["sub"].Split(new char[] { ',' });
if (parts.Length > 2)
{
opts.error("Too many commas in the argument to sub.");
}
byte address = 0;
short parameter = 0;
try
{
if (parts[0].StartsWith("0x"))
{
address = byte.Parse(parts[0].Substring(2), System.Globalization.NumberStyles.AllowHexSpecifier);
}
else
{
address = byte.Parse(parts[0]);
}
}
catch (FormatException)
{
opts.error();
}
if (parts.Length == 2)
{
try
{
if (parts[1].StartsWith("0x"))
{
parameter = short.Parse(parts[1].Substring(2), System.Globalization.NumberStyles.AllowHexSpecifier);
}
else
{
parameter = short.Parse(parts[1]);
}
}
catch (FormatException)
{
opts.error();
}
Console.Write("Restarting at subroutine " + address + " with parameter " + parameter + "...");
usc.restartScriptAtSubroutineWithParameter(address, parameter);
usc.setScriptDone(0);
}
else
{
Console.Write("Restarting at subroutine " + address + "...");
usc.restartScriptAtSubroutine(address);
usc.setScriptDone(0);
}
Console.WriteLine("");
}
else
{
opts.error();
}
}
/// <summary>
/// Moves the specified servo to the target position
/// </summary>
/// <param name="servoNo">Position where the servo is connected to the maestro</param>
/// <param name="target">How much to rotate the servo</param>
public void MoveServo(int servoNo, int target)
{
usc.setTarget((byte)servoNo, (ushort)target);
}
