// Connects to the Pololu Maestro servo controller through USB. Only one controller is
// currently expected, so this method just connects to the first controller it sees.
// Based on the 'connectToDevice' method from MaestroEasyExample in the pololu-usb-sdk.
public void ConnectToHardware()
{
try
{
DisconnectFromHardware();
// Get a list of all connected devices of this type.
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
foreach (DeviceListItem dli in connectedDevices)
{
// 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; }
uscDevice = new Usc(dli); // Connect to the device.
break; // Use first device (should only be one, anyway).
}
if (uscDevice == null)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "ConnectToHardware() failed, no servo hardware found.");
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Info, "ConnectToHardware() succeeded, connected to servo hardware.");
}
InitializeHardware();
}
catch (System.Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "Caught exception in Initialize(): " + 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>
private Usc connectToDevice(int index)
{
// Get a list of all connected devices of this type.
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
connectedDevices.Sort(deviceComparer);
// we must have all three devices online, or we will be commanding a wrong device.
if (connectedDevices.Count() != 3)
{
throw new Exception("Not all Pololu devices are online - found " + connectedDevices.Count() + " expected 3");
}
DeviceListItem dli = connectedDevices[index];
//foreach (DeviceListItem dli in connectedDevices)
{
// 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 device = new Usc(dli); // Connect to the device.
return(device); // Return the device.
}
throw new Exception("Could not find Pololu device.\r\nMake sure it's plugged into USB\r\nCheck your Device Manager.\r\nPololu Mini Maestro 12 needed.");
}
public static void Main(string[] args)
{
LCM.LCM.LCM myLCM = null;
try
{
Console.WriteLine("Starting Maestro driver...");
myLCM = new LCM.LCM.LCM("udpm://239.255.76.67:7667?ttl=1");
Console.WriteLine("LCM Connected!");
}
catch (Exception ex)
{
Console.Error.WriteLine("Ex: " + ex);
Environment.Exit(1);
}
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
Console.WriteLine("Number of MicroMaestros detected=" + connectedDevices.Count);
GoalFlags[] goals = new GoalFlags[4];
Dictionary <string, int> goalIDs = new Dictionary <string, int>();
goalIDs.Add("00032615", 3); //D
goalIDs.Add("00031531", 2); //C
goalIDs.Add("00032614", 1); //B
goalIDs.Add("00032598", 0); //A
foreach (DeviceListItem dli in connectedDevices)
{
Console.WriteLine(
"Detected Maestro: {serialNumber=" + dli.serialNumber +
", text=" + dli.text + "}");
goals[goalIDs[dli.serialNumber]] = new GoalFlags(new Usc(dli));
}
CubeRouteField field = new CubeRouteField(goals);
foreach (GoalFlags flags in goals)
{
for (int i = 0; i < 5; i++)
{
flags.setFlagPosition(i, 0);
}
}
// GoalLightsConnection conn = new GoalLightsConnection(null, "127.0.0.1", 9001, 9000);
GoalFlagsConnection conn = new GoalFlagsConnection(myLCM, field);
Console.WriteLine("Running...");
conn.Run();
}
/// <summary>
/// Connects to the device if it is found in the device list.
/// </summary>
public void TryToReconnect()
{
foreach (DeviceListItem d in Usc.getConnectedDevices())
{
if (d.serialNumber == serialNumber)
{
usc = new Usc(d);
//Log("Connected to #" + SerialNumberTextBox.Text + ".");
return;
}
}
}
// This function is called when the device connections need to be updated.
public static void UpdateDeviceConnections()
{
// Start by removing any previous connections.
disconnectDevices();
// Preparing a list of all of the currently connected devices.
List <DeviceListItem> listOfJrks = Jrk.getConnectedDevices();
List <DeviceListItem> listOfMaestros = Usc.getConnectedDevices();
// Store the number of connected devices in these public
// variables for use throughout all classes.
NumOfConnectedJrks = listOfJrks.Count;
NumOfConnectedMaestros = listOfMaestros.Count;
// Attempting to connect to all available devices.
if (NumOfConnectedMaestros > 0)
{
item1 = listOfMaestros[0];
usc = new Usc(item1);
}
else
{
MessageBox.Show("There are no Maestros connected, so no operations can be done. Please connect to a Maestro and try again.");
return;
}
if (NumOfConnectedJrks > 0)
{
item = listOfJrks[0];
Jrk1 = new Jrk(item);
}
else
{
MessageBox.Show("There are no Jrks connected, so no operations can be done. Please connect to a Jrk and try again.");
return;
}
if (NumOfConnectedJrks > 1)
{
item = listOfJrks[1];
Jrk2 = new Jrk(item);
}
if (NumOfConnectedJrks > 2)
{
item = listOfJrks[2];
Jrk3 = new Jrk(item);
}
if (NumOfConnectedJrks > 3)
{
item = listOfJrks[3];
Jrk4 = new Jrk(item);
}
}
/// <summary>
/// Get the serial number of the first connected Maestro, since this is
/// probably what you want to connect to.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void MainWindow_Shown(object sender, EventArgs e)
{
var device_list = Usc.getConnectedDevices();
if (device_list.Count > 0)
{
SerialNumberTextBox.Text = device_list[0].serialNumber;
ConnectButton.Focus();
}
else
{
SerialNumberTextBox.Focus();
}
}
/// <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>
Usc connectToDevice()
{
// Get a list of all connected devices of this type.
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
foreach (DeviceListItem dli in connectedDevices)
{
// 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 device = new Usc(dli); // Connect to the device.
return(device); // Return the device.
}
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).");
}
static void listDevices()
{
List <DeviceListItem> list = Usc.getConnectedDevices();
if (list.Count == 1)
{
Console.WriteLine("1 " + Usc.englishName + " device found:");
}
else
{
Console.WriteLine(list.Count + " " + Usc.englishName + " devices found:");
}
foreach (DeviceListItem item in list)
{
Console.WriteLine(item.text);
}
}
/// <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());
}
}
/// <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>
public Usc connectToDevice(int index)
{
// Get a list of all connected devices of this type.
List <DeviceListItem> connectedDevices = Usc.getConnectedDevices();
connectedDevices.Sort(deviceComparer);
DeviceListItem dli = connectedDevices[index];
//foreach (DeviceListItem dli in connectedDevices)
{
// 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 device = new Usc(dli); // Connect to the device.
return(device); // Return the device.
}
throw new Exception("Could not find Pololu device.\r\nMake sure it's plugged into USB\r\nCheck your Device Manager.\r\nPololu Mini Maestro 12 needed.");
}
private void refreshAvailablePololuMaestroList()
{
AvailablePololuMaestroList.Clear();
AvailablePololuMaestroList.AddRange(Usc.getConnectedDevices());
}
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();
}
}
