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 + "."); } }