Ejemplo n.º 1
0
 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);
 }
Ejemplo n.º 2
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");
            };
        }
Ejemplo n.º 3
0
        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);
                    }
                }
            }
        }
Ejemplo n.º 4
0
 /// <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))
 {
 }
Ejemplo n.º 5
0
 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;
 }
Ejemplo n.º 6
0
        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;
                }
            }
        }
Ejemplo n.º 7
0
        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();
        }
Ejemplo n.º 8
0
        /// <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;
                }
            }
        }
Ejemplo n.º 9
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;
         }
     }
 }
Ejemplo n.º 10
0
        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 + ".");
            }
        }