// 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);
}
}
static PololuMiniUsb()
{
List<DeviceListItem> device_list = Usc.getConnectedDevices();
if (device_list.Count > 0)
{
serialNumber = device_list[0].serialNumber;
usc = new Usc(device_list[0]);
settings = usc.getUscSettings();
Console.WriteLine("Pololu connectée");
List<int> servos = new List<int>();
servos.Add(1);
servos.Add(0);
servos.Add(3);
servos.Add(2);
servos.Add(5);
servos.Add(8);
servos.Add(6);
servos.Add(7);
connected = true;
for (int i = 0; i < servos.Count; i++)
{
setMin(servos[i], 256);
setMax(servos[i], 16320);
}
}
else
{
Console.WriteLine("Pololu non connectée");
}
}
//
// Disconnect the servos
//
public void Dispose()
{
if (_device == null)
return;
try { _device.Dispose(); }
catch (Exception) { }
finally { _device = null; }
}
public Usc Connect()
{
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 servo driver device. Make sure it is plugged in to USB");
}
public void Initialize()
{
List<DeviceListItem> list = Usc.getConnectedDevices();
if (list.Count == 1)
{
Driver = new Usc(list[0]);
}
else if (list.Count == 0)
{
Logger.Log(this, "there are no connected USC devices - servo driver can't start", 2);
}
else //more than 1 device
{
Logger.Log(this, "there are more than 1 USC devices - trying to connect last of them", 2);
Driver = new Usc(list[list.Count - 1]); //last device
//TODO: add device recognising
}
}
private double lastThrottleInPercentsWantedToBeSet = 0.0; //this throttle could not been set because effectors could be not active
protected override void Initialize()
{
List<DeviceListItem> list = Usc.getConnectedDevices();
if (list.Count == 1)
{
Driver = new Usc(list[0]);
this.overallState = DeviceOverallState.OK;
}
else if (list.Count == 0)
{
Logger.Log(this, "there are no connected USC devices - servo driver can't start", 2);
this.overallState = DeviceOverallState.Error;
}
else //more than 1 device
{
Logger.Log(this, "there are more than 1 USC devices - trying to connect last of them", 2);
Driver = new Usc(list[list.Count - 1]); //last device
//TODO: add device recognising
this.overallState = DeviceOverallState.Warrning;
}
}
private void TryToDisconnect()
{
if (usc == null)
{
Log("Connecting stopped.");
return;
}
try
{
Log("Disconnecting...");
usc.Dispose(); // Disconnect
}
catch (Exception e)
{
Log(e);
Log("Failed to disconnect cleanly.");
}
finally
{
// do this no matter what
usc = null;
Log("Disconnected from #" + SerialNumberTextBox.Text + ".");
}
}
Usc connectToDevice()
{
List<DeviceListItem> connectedDevices=Usc.getConnectedDevices();
foreach (DeviceListItem dli in connectedDevices)
{
Usc device=new Usc(dli);
return device;
}
throw new Exception("Could not find device");
}
/// <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).");
}
private void DisconnectFromDevice()
{
_state.Connected = false;
if (_setServoPorts != null)
{
foreach (var servoPort in _setServoPorts)
servoPort.Clear();
}
_setServoPorts = null;
if (_setChannelThrotling != null)
{
foreach (var port in _setChannelThrotling)
port.Clear();
}
_setChannelThrotling = null;
if (_usc != null)
{
try
{
_usc.Dispose();
_usc = null;
}
catch (Exception ex)
{
LogError("Error disconnecting device", ex);
throw;
}
}
}
private bool ConnectToDevice()
{
DisconnectFromDevice();
List<usbWrapper.DeviceListItem> devices = Usc.getConnectedDevices();
if (devices.Count == 0)
{
LogWarning(LogGroups.Console, "No device connected.");
return false;
}
usbWrapper.DeviceListItem dev = null;
if (!string.IsNullOrEmpty(_state.SerialNumber))
dev = devices.Find(d => d.serialNumber == _state.SerialNumber);
if (dev == null)
{
// user first device
dev = devices[0];
_state.SerialNumber = dev.serialNumber;
}
LogVerbose("Connecting to device " + dev.text);
try
{
_usc = new Usc(dev);
}
catch (Exception ex)
{
LogError("Could not connect to device", ex);
throw;
}
LogInfo(string.Format("Connected to device #{0} (firmware {1}) with {2} servo",
_usc.getSerialNumber(), _usc.firmwareVersionString, _usc.servoCount));
_state.Connected = true;
if (_state.Channels == null || _state.Channels.Count != _usc.servoCount)
{
_state.Channels = new List<ChannelInfo>(_usc.servoCount);
for (int i = 0; i < _usc.servoCount; i++)
{
_state.Channels.Add(new ChannelInfo {Index = i});
}
}
UpdateChannelSettings();
_setServoPorts = new Port<SetChannel>[_usc.servoCount];
_setChannelThrotling = new Port<DateTime>[_usc.servoCount];
for (int i = 0; i < _usc.servoCount; i++)
{
_setServoPorts[i] = new Port<SetChannel>();
_setChannelThrotling[i] = new Port<DateTime>();
RegisterSetChannelReceiver(i);
}
return true;
}
/// <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.");
}
/// <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>
/// 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>
/// This function will be called once every 100 ms to do an update.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void UpdateTimer_Tick(object sender, EventArgs e)
{
if (usc == null)
{
// Display a message in the position box
PositionTextBox.Text = "(disconnected)";
// Try connecting to a device.
try
{
TryToReconnect();
}
catch (Exception e2)
{
Log(e2);
Log("Failed connecting to #" + SerialNumberTextBox.Text + ".");
usc = null;
}
}
else
{
// Update the GUI and the device.
try
{
DisplayPosition();
if (ActivateCheckBox.Checked)
RunSequence();
}
catch (Exception e2)
{
// If any exception occurs, log it, set usc to null, and keep trying..
Log(e2);
Log("Disconnected from #"+SerialNumberTextBox.Text+".");
usc = null;
}
}
}
/// <summary>
/// Connects to the device if it is found in the device list.
/// </summary>
private void TryToReconnect()
{
foreach (DeviceListItem d in Usc.getConnectedDevices())
{
if (d.serialNumber == SerialNumberTextBox.Text)
{
usc = new Usc(d);
Log("Connected to #" + SerialNumberTextBox.Text + ".");
return;
}
}
}
/// <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.");
}
// Disconnects from the Pololu Maestro servo controller.
// Based on the 'TryToDisconnect' method from MaestroAdvancedExample in the pololu-usb-sdk.
public void DisconnectFromHardware()
{
lock (uscLock)
{
if (uscDevice == null)
{
// Already disconnected
return;
}
try
{
uscDevice.Dispose(); // Disconnect
}
catch (Exception ex)
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Error, "DisconnectFromHardware failed to cleaning disconnect the servo hardware: " + ex.Message);
}
finally
{
// do this no matter what
uscDevice = null;
}
}
}
public void ConnectHardware()
{
if (usbdevice == null) usbdevice = connectToDevice();
}
public GoalFlags(Usc maestro)
{
this.maestro = maestro;
}
public ServoDriver()
{
_device = Connect();
}
