public Motor(MotorInfo info, Jrk controller)
{
if (controller == null)
{
throw new InvalidInputException("Motor must have Jrk controller attached!");
}
this.info = info;
this.info.controller = controller;
this.info.controller.setJrkParameter(jrkParameter.PARAMETER_MOTOR_COAST_WHEN_OFF, 0);//set to brake when power is turned off
SetSpeed(0);
}
static void displayStatus(Jrk jrk)
{
Console.Write(
"Serial number: " + jrk.getSerialNumber() + "\n" +
"Firmware version: " + jrk.firmwareVersionString + "\n"
);
storeCurrentCalibration();
jrkVariables vars = jrk.getVariables();
Console.Write(
"Variables:\n" +
" Input: " + vars.input + "\n" +
" Target: " + vars.target + "\n" +
" Feedback: " + vars.feedback + "\n" +
" Scaled feedback: " + vars.scaledFeedback + "\n" +
" Error: " + (vars.scaledFeedback - vars.target) + "\n" +
" Integral: " + vars.errorSum + "\n" +
" Duty cycle target: " + vars.dutyCycleTarget + "\n" +
" Duty cycle: " + vars.dutyCycle + "\n" +
" Current (mA): " + currentToMilliamps(vars.current, vars.dutyCycle) + "\n"
);
if (vars.errorFlagBits == 0)
{
Console.WriteLine("No errors.");
}
else
{
Console.WriteLine("Errors:");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_AWAITING_COMMAND)) Console.WriteLine(" Awaiting command");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_NO_POWER)) Console.WriteLine(" No power");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_MOTOR_DRIVER)) Console.WriteLine(" Motor driver error");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_INVALID)) Console.WriteLine(" Input invalid");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_INPUT_DISCONNECT)) Console.WriteLine(" Input disconnect");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_FEEDBACK_DISCONNECT)) Console.WriteLine(" Feedback disconnect");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_MAXIMUM_CURRENT_EXCEEDED)) Console.WriteLine(" Max. current exceeded");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_SIGNAL)) Console.WriteLine(" Serial signal error");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_OVERRUN)) Console.WriteLine(" Serial overrun");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_BUFFER_FULL)) Console.WriteLine(" Serial RX buffer full");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_CRC)) Console.WriteLine(" Serial CRC error");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_PROTOCOL)) Console.WriteLine(" Serial protocol error");
if (1 == (1 & vars.errorFlagBits >> (byte)jrkError.ERROR_SERIAL_TIMEOUT)) Console.WriteLine(" Serial timeout error");
};
}
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);
}
}
}
}
/// <summary>
/// Builds a Motor with the appropriate specifications. Only the horizontal angle and elevation angle have direct getters and setters.
/// </summary>
/// <param name="xLoc"></param>
/// <param name="yLoc"></param>
/// <param name="zLoc"></param>
/// <param name="horizontalAngle"></param>
/// <param name="elevationAngle"></param>
/// <param name="reversable"></param>
/// <param name="controller"></param>
public Motor(int xLoc, int yLoc, int zLoc, int horizontalAngle, int elevationAngle, bool reversable, Jrk controller, String name)
: this(new MotorInfo(null, xLoc, yLoc, zLoc, horizontalAngle, elevationAngle, reversable, controller, name))
{
}
public MotorInfo(string serial, int xPos, int yPos, int zPos, int horzTheta, int vertTheta, bool reversable, Jrk controller, string name)
{
this.serial = serial;
this.xPos = xPos;
this.yPos = yPos;
this.zPos = zPos;
this.horzTheta = horzTheta;
this.vertTheta = vertTheta;
this.reversable = reversable;
this.controller = controller;
this.name = name;
}
static void streamVariables(Jrk jrk, Dictionary<String, String> opts)
{
// Determine what interval to use (the time between each reading).
int interval = 20;
if (opts.ContainsKey("interval"))
{
try
{
interval = int.Parse(opts["interval"]);
if (interval < 0)
{
throw new Exception("Value must be a non-negative whole number.");
}
}
catch (Exception exception)
{
throw new Exception("Invalid interval parameter \"" + opts["interval"] + "\".", exception);
}
}
// Determine the output format.
if (opts.ContainsKey("format"))
{
// The user specified the format string completely.
streamFormat = opts["format"].Replace("\\t", "\t");
}
else
{
string separator = ",";
if (opts.ContainsKey("separator"))
{
// The user just specified the separator
separator = opts["separator"];
switch (separator)
{
case "space": separator = " "; break;
case "\\t":
case "tab": separator = "\t"; break;
case "comma": separator = ","; break;
}
}
if (separator == "\t")
{
// If the separator is a tab, then we don't need to have padding spaced.
streamFormat = "{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t{6}\t{7}\t{8}\t{9}\t{10}";
}
else
{
// If the separator is something else, then we want padding to make each
// field be a fixed width.
streamFormat = "{0,6}" + separator +
"{1,6}" + separator +
"{2,4}" + separator +
"{3,4}" + separator +
"{4,4}" + separator +
"{5,4}" + separator +
"{6,6}" + separator +
"{7,6}" + separator +
"{8,4}" + separator +
"{9,6}" + separator +
"{10,4}";
}
}
// Determine the limit.
if (opts.ContainsKey("limit"))
{
try
{
streamLineCountLimit = UInt32.Parse(opts["limit"]);
}
catch(Exception exception)
{
throw new Exception("Invalid limit parameter \"" + opts["limit"] + "\".", exception);
}
}
// Print the header line if the user wanted it
if (!opts.ContainsKey("noheader"))
{
Console.WriteLine(streamFormat,
"Time (ms)",
"PID Period Count",
"Input",
"Target",
"Feedback",
"Scaled feedback",
"Integral",
"Duty cycle target",
"Duty cycle",
"Current (mA)",
"Error code",
"PID period exceeded");
}
// Prepare to process the current readings the Jrk will be sending us.
storeCurrentCalibration();
if (interval == 0)
{
// Interval is zero so the user just wants the data as fast as possible.
while (true)
{
streamPrintReading(DateTime.Now, jrk.getVariables());
}
}
else
{
// The three timer classes provided by the .NET framework do not
// provide accurate timing so instead we poll DateTime.Now.
// This takes more CPU power than the timer method, but we try
// to minimize the CPU use by sleeping between readings.
DateTime now = DateTime.Now;
DateTime nextUpdateTime = now;
while (true)
{
if (nextUpdateTime <= now)
{
// Get the reading from the Jrk over USB (should take about .2 ms).
jrkVariables vars = jrk.getVariables();
streamPrintReading(now, vars);
while (nextUpdateTime <= DateTime.Now)
{
nextUpdateTime = nextUpdateTime.AddMilliseconds(interval);
}
// Conserve CPU power by sleeping
if (interval > 4 && !opts.ContainsKey("nosleep"))
{
Thread.Sleep(interval - 4);
}
}
now = DateTime.Now;
}
}
}
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();
}
/// <summary>
/// This function will be called once every 100 ms to do an update.
/// </summary>
void UpdateTimer_Tick(object sender, EventArgs eventArgs)
{
if (jrk == null)
{
// Display a message in the textboxes.
TargetTextBox.Text = "(disconnected)";
ScaledFeedbackTextBox.Text = "(disconnected)";
// Try connecting to a device.
try
{
TryToReconnect();
}
catch (Exception e)
{
Log(e);
Log("Failed connecting to #" + SerialNumberTextBox.Text + ".");
jrk = null;
}
}
else
{
// Update the GUI and the device.
try
{
DisplayStatus();
if (ActivateCheckBox.Checked)
RunSequence();
}
catch (Exception e)
{
// If any exception occurs, log it, set jrk to null, and keep trying..
Log(e);
Log("Disconnected from #"+SerialNumberTextBox.Text+".");
jrk = null;
}
}
}
/// <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;
}
}
}
void TryToDisconnect()
{
if (jrk == null)
{
Log("Connecting stopped.");
return;
}
try
{
Log("Disconnecting...");
jrk.disconnect();
}
catch (Exception e)
{
Log(e);
Log("Failed to disconnect cleanly.");
}
finally
{
// do this no matter what
jrk = null;
Log("Disconnected from #" + SerialNumberTextBox.Text + ".");
}
}
