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 } } } }
static void Main() { List<DeviceListItem> list = Jrk.getConnectedDevices(); while(true) { string line; while((line=Console.ReadLine())!=null) { string[] p = line.Split(); if(p.Length==1) { ushort id = Convert.ToUInt16(p[0]); if(id>=list.Count){Console.WriteLine(-1);continue;}; jrk = new Jrk(list[id]); double current = 0.0; for(int i=0; i<100; i++) current += jrk.getVariables().current; Console.WriteLine(current); jrk.disconnect(); } if(p.Length==2) { ushort id = Convert.ToUInt16(p[0]); if(id>=list.Count)continue; ushort v = Convert.ToUInt16(Convert.ToDouble(p[1])*40.95); if(v>4095)v=4095; jrk = new Jrk(list[id]); jrk.setTarget(v); jrk.disconnect(); } } } }
// Function used for converting serial data to readable current data. public static int currentToMilliamps(Jrk jrk, byte rawCurrent, short dutyCycle) { int current = rawCurrent; if (dutyCycle > 0) { current *= currentCalibrationForward; } else { current *= currentCalibrationReverse; } if (jrk.divideCurrent) { if (dutyCycle == 0) { current = 0; } else { current = (current * 600) / dutyCycle; } } return(current); }
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 + "."); } }
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); }
/// <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; } } }
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; }
// 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); } } } }
/// <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; } } }
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(); }
public const int RAMP_PAUSE_MILLISECONDS = 5; //pause between rampage passes #endregion #region constructors /// <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)) { }
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"); } }; }
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(); }