// Sets the speed of the specified servo, in the servo controller hardware. This speed
// value is first bounded within the servo's min/max speed limits, and a warning is
// logged if a speed outside of these limits was specified.
private void SetServoSpeed(Servo servo, long speed)
{
if (speed < servo.speedLimitMin)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to minimum limit " + servo.speedLimitMin.ToString());
// Bound to this limit.
speed = servo.speedLimitMin;
}
if (speed > servo.speedLimitMax)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Warning, "Requested servo " + servo.index.ToString() + " speed " + speed.ToString() + " bound to maximum limit " + servo.speedLimitMax.ToString());
// Bound to this limit.
speed = servo.speedLimitMax;
}
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "Setting servo " + servo.index.ToString() + " speed to " + speed.ToString());
try
{
// Send this value to the hardware.
uscDevice.setSpeed((byte)servo.index, (ushort)speed);
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in SetServoSpeed(): " + ex.Message);
}
}
/// <summary>
/// Connects to a Maestro using native USB and returns the Usc object
/// representing that connection. When you are done with the
/// connection, you should close it using the Dispose() method so that
/// other processes or functions can connect to the device later. The
/// "using" statement can do this automatically for you.
/// </summary>
void connectToDevice()
{
try{
// Get a list of all connected devices of this type.
if (usbdevice == null)
{
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
foreach (DeviceListItem dli in connectedDevices)
{
if ((device == null))//|| (this.device == dli.serialNumber)
{
// If you have multiple devices connected and want to select a particular
// device by serial number, you could simply add a line like this:
// if (dli.serialNumber != "00012345"){ continue; }
Usc uscdevice = new Usc(dli); // Connect to the device.
usbdevice = uscdevice;
// Return the device.
}
}
}
if (usbdevice != null)
{
usbdevice.clearErrors();
var settings = usbdevice.getUscSettings();
settings.channelSettings[channel].mode = ChannelMode.Servo;
Console.WriteLine("Set Startup Position Servo " + channel + " to " + settings.channelSettings[channel].home);
settings.servoPeriod = 20;
usbdevice.setUscSettings(settings, false);
// usbdevice.setAcceleration(channel, 0);
usbdevice.setSpeed(channel, 0);
}
else
{
throw new Exception("Could not find device. Make sure it is plugged in to USB " +
"and check your Device Manager (Windows) or run lsusb (Linux).");
}
}catch (Exception ex) {
Console.WriteLine("Fehler beim Verbinden mit dem Servo " + ex.Message + ex.StackTrace.ToString());
}
}
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();
}
}
