public static void InitializeMotors()
{
//get all the connected motors and instantiate them
List <DeviceListItem> devices = Jrk.getConnectedDevices();
Motor current;
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
current = new Motor(info, new Jrk(d));
motors.Add(current);
if (info.horzTheta == 0 && info.vertTheta == 0)
{
xMotors.Add(current);
}
if (info.horzTheta != 0 && info.vertTheta == 0)
{
yMotors.Add(current);
}
if (info.horzTheta == 0 && info.vertTheta != 0)
{
zMotors.Add(current);
}
//yaw if horizontal angle
}
}
}
}
/// <summary>
/// Connects to the device if it is found in the device list.
/// </summary>
void TryToReconnect()
{
foreach (DeviceListItem d in Jrk.getConnectedDevices())
{
if (d.serialNumber == SerialNumberTextBox.Text)
{
jrk = new Jrk(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>
/// When the program starts, this function is called.
/// It gets the serial number of the first connected Jrk,
/// since this is probably what you want to connect to.
/// </summary>
void MainWindow_Shown(object sender, EventArgs e)
{
var deviceList = Jrk.getConnectedDevices();
if (deviceList.Count > 0)
{
SerialNumberTextBox.Text = deviceList[0].serialNumber;
ConnectButton.Focus();
}
else
{
SerialNumberTextBox.Focus();
}
}
/// <summary>
/// Prints a list of the serial numbers of all Jrks connected to
/// the computer.
/// </summary>
static void listDevices()
{
List <DeviceListItem> list = Jrk.getConnectedDevices();
if (list.Count == 1)
{
Console.WriteLine("1 jrk found:");
}
else
{
Console.WriteLine(list.Count + " " + Jrk.shortProductName + "s found:");
}
foreach (DeviceListItem item in list)
{
Console.WriteLine(item.text);
}
}
public void runTests()
{
//get all the connected motors and instantiate them
List <DeviceListItem> devices = Jrk.getConnectedDevices();
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
motors.Add(new Motor(info, new Jrk(d)));
}
}
}
//run random tests
// do
{
foreach (Motor m in motors)
{
System.Console.WriteLine("Testing motor " + m.info.serial + ".");
testing = m;
//TestMotor(m);
new Thread(new DirectMotorLinkTest().TestMotor).Start();
System.Console.WriteLine();
System.Threading.Thread.Sleep(500);
if (System.Console.KeyAvailable)
{
break;
}
}
System.Threading.Thread.Sleep(10000);
}// while (!System.Console.KeyAvailable);
System.Console.WriteLine("Press Enter to quit.");
System.Console.ReadLine();
}
static void Main(string[] args)
{
IPressureSensor pSensor = new ArduinoPressureSensor();
System.Threading.Thread.Sleep(2000);
pSensor.calibrate();
Console.WriteLine("RoboSub Console Output");
Jrk jrk = null;
var deviceList = Jrk.getConnectedDevices();
if (deviceList.Count > 0)
{
Console.WriteLine("Connected to USB port " + deviceList[0].serialNumber);
jrk = new Jrk(deviceList[0]);
}
else
{
Console.WriteLine("Can't find any motor");
}
while (true)
{
Console.SetCursorPosition(0, 2);
Console.WriteLine("Depth: {0:0.00} feet ", pSensor.getDepth());
Console.WriteLine("Pressure: {0:0.00} psi ", pSensor.Psi);
if (deviceList.Count > 0)
{
if (pSensor.getDepth() > 0.4)
{
jrk.setTarget(2548);
}
else
{
jrk.setTarget(2048);
}
}
}
}
public void boxTest()
{
//get all the connected motors and instantiate them
List <DeviceListItem> devices = Jrk.getConnectedDevices();
Motor current;
foreach (DeviceListItem d in devices)
{
System.Console.WriteLine("Checking device " + d.serialNumber);
//if the found device matches the expected serial number, proceed with initialization
foreach (MotorInfo info in HandCodeMotorConstants.handCodedMotors)
{
if (info.serial.Equals(d.serialNumber))
{
System.Console.WriteLine("Found motor " + info.serial);
current = new Motor(info, new Jrk(d));
motors.Add(current);
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
xMotors.Add(current);
}
if (info.horzTheta < 1.1 && info.horzTheta > 0.9 && info.vertTheta < 0.1 && info.vertTheta > -0.1)
{
yMotors.Add(current);
}
if (info.horzTheta < 0.1 && info.horzTheta > -0.1 && info.vertTheta < 1.1 && info.vertTheta > 0.9)
{
zMotors.Add(current);
}
}
}
}
int time = 2000;//how many milliseconds to run for
//wait before starting tests
//System.Threading.Thread.Sleep(time);
//System.Console.WriteLine("Starting box drive test");
//System.Console.WriteLine("Diving.");
//dive();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Forward.");
//driveForward();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Backward.");
//driveBackwards();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
ConsoleKey key = ConsoleKey.P;
System.Console.WriteLine("Ready for input");
while (key != ConsoleKey.Q)
{
key = Console.ReadKey().Key;
time = 2000;//in milliseconds
switch (key)
{
case ConsoleKey.W:
System.Console.WriteLine("\nDriving Forward");
driveForward();
break;
case ConsoleKey.S:
System.Console.WriteLine("\nDriving Backwards");
driveBackwards();
break;
case ConsoleKey.R:
System.Console.WriteLine("\nSurfacing");
surface();
break;
case ConsoleKey.F:
System.Console.WriteLine("\nDiving");
dive();
break;
case ConsoleKey.P:
System.Console.WriteLine("\nKilling Peasants");
foreach (Motor m in motors)
{
System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
m.KillMotors();
}
break;
}
}
//System.Console.WriteLine("Surface.");
//surface();
//System.Threading.Thread.Sleep(time);
//foreach (Motor m in motors)
//{
// System.Console.WriteLine("Stopping motor " + m.info.serial + ".");
// m.KillMotors();
//}
//System.Console.WriteLine("Press Enter to quit.");
//System.Console.ReadLine();
}
static void MainWithExceptions(string[] args)
{
// If no arguments are given, just show the help message.
if (args.Length == 0)
{
Console.Write(helpMessage());
Environment.Exit(2);
}
// Parse the arguments.
Dictionary <String, String> opts = new Dictionary <string, string>();
string name = null;
foreach (string rawArg in args)
{
string arg = rawArg;
// Transform the short names in to the long names.
switch (arg)
{
case "-l": arg = "--list"; break;
case "-d": arg = "--device"; break;
case "-s": arg = "--status"; break;
}
Match m = Regex.Match(arg, "^--(.*)");
if (m.Success)
{
name = m.Groups[1].ToString();
opts[name] = ""; // start it off with no string value
}
else if (name != null)
{
// This argument is right after a -- argument, so this argument
// is its value.
opts[name] = arg;
name = null;
}
else
{
throw new ArgumentException("Unexpected argument \"" + arg + "\".");
}
}
if (opts.ContainsKey("list"))
{
if (args.Length > 1)
{
throw new ArgumentException("If --list is present, it must be the only option.");
}
listDevices();
return;
}
// Otherwise, we have to connect to a device.
List <DeviceListItem> list = Jrk.getConnectedDevices();
// Make sure there is a device available..
if (list.Count == 0)
{
throw new Exception("No jrks found.");
}
DeviceListItem item = null;
if (!opts.ContainsKey("device"))
{
// No serial number specified: connect to the first item in the list.
item = list[0];
}
else
{
// Serial number specified.
// Remove the leading # sign. It is not standard to put it there,
// but if someone writes it, this 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)
{
throw new Exception("Could not find a jrk with serial number " + opts["device"] + ".\n" +
"To list devices, use the --list option.");
}
}
// Connect to the device.
jrk = new Jrk(item);
if (opts.ContainsKey("bootloader"))
{
jrk.startBootloader();
return;
}
if (opts.ContainsKey("restoredefaults"))
{
jrk.setJrkParameter(jrkParameter.PARAMETER_INITIALIZED, 0xFF);
jrk.reinitialize();
Thread.Sleep(1000);
}
if (opts.ContainsKey("configure"))
{
string filename = opts["configure"];
Stream stream = File.Open(filename, FileMode.Open);
StreamReader sr = new StreamReader(stream);
ConfigurationFile.load(sr, jrk);
sr.Close();
stream.Close();
jrk.reinitialize();
}
if (opts.ContainsKey("getconf"))
{
string filename = opts["getconf"];
Stream stream = File.Open(filename, FileMode.Create);
StreamWriter sw = new StreamWriter(stream);
ConfigurationFile.save(sw, jrk);
sw.Close();
stream.Close();
}
if (opts.ContainsKey("clearerrors"))
{
jrk.clearErrors();
}
if (opts.ContainsKey("target"))
{
UInt16 target = stringToU12(opts["target"]);
jrk.setTarget(target);
}
if (opts.ContainsKey("run"))
{
UInt16 target = jrk.getVariables().target;
jrk.setTarget(target);
}
if (opts.ContainsKey("stop"))
{
jrk.motorOff();
}
if (opts.ContainsKey("status"))
{
displayStatus(jrk);
}
if (opts.ContainsKey("stream"))
{
streamVariables(jrk, opts);
}
jrk.disconnect();
}
