Beispiel #1
0
        public void ServoSweepParams(int servoNumber, double sweepMin, double sweepMax, double sweepStartPos, double sweepStep, bool initialDirectionUp, double sweepRate)
        {
            try
            {
                Tracer.Trace("PM Command::ServoSweepParams(" + servoNumber + "," + sweepMin + "," + sweepMax + "," + sweepStep + "," + sweepRate + ")");

                sweepMin = ProximityBoard.toWithinServoRangeMks(sweepMin);        // cut it to the allowed limits
                sweepMax = ProximityBoard.toWithinServoRangeMks(sweepMax);

                if (!myDeviceDetected)
                {
                    myDeviceDetected = FindTheHid(vendorId, productId);
                }

                if (myDeviceDetected)
                {
                    //  Set the size of the Output report buffer.

                    byte[] outputReportBuffer = new byte[MyHid.Capabilities.OutputReportByteLength];

                    //  Store the report ID (command) in the first byte of the buffer:

                    outputReportBuffer[0] = OUTPUT_REPORT_ID;                             // will turn out on the board as Output Report ID

                    outputReportBuffer[1] = (byte)ProximityBoardCommand.ServoSweepParams; // will be analyzed in a switch

                    outputReportBuffer[2] = (byte)servoNumber;                            // which servo

                    int iSweepMin   = ProximityBoard.mksToServoTarget(sweepMin);
                    int iSweepMax   = ProximityBoard.mksToServoTarget(sweepMax);
                    int iSweepStart = ProximityBoard.mksToServoTarget(sweepStartPos);
                    int iSweepStep  = (int)sweepStep;
                    int iSweepRate  = (int)sweepRate;

                    intToBuffer(outputReportBuffer, 3, (UInt16)iSweepMin);
                    intToBuffer(outputReportBuffer, 5, (UInt16)iSweepMax);
                    intToBuffer(outputReportBuffer, 7, (UInt16)iSweepStart);
                    intToBuffer(outputReportBuffer, 9, (UInt16)iSweepStep);
                    intToBuffer(outputReportBuffer, 11, (UInt16)iSweepRate);
                    outputReportBuffer[13] = (byte)(initialDirectionUp ? 1 : -1);

                    ReadAndWriteToDevice(outputReportBuffer, INPUT_REPORT_ID);
                    //WriteToDevice(outputReportBuffer);
                }
                else
                {
                    Tracer.Error("FindTheHid() unsuccessful, skipped the ServoSweepParams command");
                }
            }
            catch (Exception ex)
            {
                Tracer.Error(ex);
                throw;
            }
        }
        public AsyncInputFrameArgs(int servo1target, int servo2target, int ping1value, int ping2value, bool fpss, SensorsState sensState)
        {
            timestamp = DateTime.Now.Ticks;

            fromPingScanStop = fpss;

            dPos1Mks = ProximityBoard.servoTargetToMks(servo1target);
            dPos2Mks = ProximityBoard.servoTargetToMks(servo2target);

            dPing1DistanceM = ProximityBoard.pingValueToDistanceM(ping1value);
            dPing2DistanceM = ProximityBoard.pingValueToDistanceM(ping2value);

            sensorsState = sensState;
        }
Beispiel #3
0
        /// <summary>
        /// returns distance in meters
        /// </summary>
        /// <param name="pingUnitNumber"></param>
        /// <returns></returns>
        public double PingDistanceGet(int pingUnitNumber)
        {
            double dPingDistanceM = 0;

            try
            {
                Tracer.Trace("PM Command::PingDistanceGet(" + pingUnitNumber + ")");

                if (!myDeviceDetected)
                {
                    myDeviceDetected = FindTheHid(vendorId, productId);
                }

                if (myDeviceDetected)
                {
                    //  Set the size of the Output report buffer.

                    byte[] outputReportBuffer = new byte[MyHid.Capabilities.OutputReportByteLength];

                    //  Store the report ID (command) in the first byte of the buffer:

                    outputReportBuffer[0] = OUTPUT_REPORT_ID;                            // will turn out on the board as Output Report ID

                    outputReportBuffer[1] = (byte)ProximityBoardCommand.PingDistanceGet; // will be analyzed in a switch

                    outputReportBuffer[2] = (byte)pingUnitNumber;                        // which servo

                    byte[] inputReportBuffer = ReadAndWriteToDevice(outputReportBuffer, INPUT_REPORT_ID);

                    // data bytes 1 and 2 of the Input Report contain ping response value:

                    int pingResponseValue = intFromBuffer(inputReportBuffer, 1);

                    dPingDistanceM = ProximityBoard.pingValueToDistanceM(pingResponseValue);
                }
                else
                {
                    Tracer.Error("FindTheHid() unsuccessful, skipped the PingDistanceGet command");
                }
            }
            catch (Exception ex)
            {
                Tracer.Error(ex);
                throw;
            }

            return(dPingDistanceM);
        }
Beispiel #4
0
        public double ServoPositionGet(int servoNumber)
        {
            double dPosMks = 0;

            try
            {
                Tracer.Trace("PM Command::ServoPositionGet(" + servoNumber + ")");

                if (!myDeviceDetected)
                {
                    myDeviceDetected = FindTheHid(vendorId, productId);
                }

                if (myDeviceDetected)
                {
                    //  Set the size of the Output report buffer.

                    byte[] outputReportBuffer = new byte[MyHid.Capabilities.OutputReportByteLength];

                    //  Store the report ID (command) in the first byte of the buffer:

                    outputReportBuffer[0] = OUTPUT_REPORT_ID;                             // will turn out on the board as Output Report ID

                    outputReportBuffer[1] = (byte)ProximityBoardCommand.ServoPositionGet; // will be analyzed in a switch

                    outputReportBuffer[2] = (byte)servoNumber;                            // which servo

                    byte[] inputReportBuffer = ReadAndWriteToDevice(outputReportBuffer, INPUT_REPORT_ID);

                    // data bytes 1 and 2 of the Input Report contain servo position:

                    int servotarget = intFromBuffer(inputReportBuffer, 1);

                    dPosMks = ProximityBoard.servoTargetToMks(servotarget);
                }
                else
                {
                    Tracer.Error("FindTheHid() unsuccessful, skipped the ServoPositionGet command");
                }
            }
            catch (Exception ex)
            {
                Tracer.Error(ex);
                throw;
            }

            return(dPosMks);
        }
Beispiel #5
0
        public void ServoPositionSet(int servoNumber, double dPosMks)
        {
            try
            {
                Tracer.Trace("PM Command::ServoPositionSet(" + servoNumber + "," + dPosMks + ")");

                dPosMks = ProximityBoard.toWithinServoRangeMks(dPosMks);        // cut it to the allowed limits

                if (!myDeviceDetected)
                {
                    myDeviceDetected = FindTheHid(vendorId, productId);
                }

                if (myDeviceDetected)
                {
                    //  Set the size of the Output report buffer.

                    byte[] outputReportBuffer = new byte[MyHid.Capabilities.OutputReportByteLength];

                    //  Store the report ID (command) in the first byte of the buffer:

                    outputReportBuffer[0] = OUTPUT_REPORT_ID;                             // will turn out on the board as Output Report ID

                    outputReportBuffer[1] = (byte)ProximityBoardCommand.ServoPositionSet; // will be analyzed in a switch

                    outputReportBuffer[2] = (byte)servoNumber;                            // which servo

                    int servotarget = ProximityBoard.mksToServoTarget(dPosMks);

                    // data bytes 0 and 1 of the Output Report - hid_report_out[]:
                    intToBuffer(outputReportBuffer, 3, (UInt16)servotarget);

                    ReadAndWriteToDevice(outputReportBuffer, INPUT_REPORT_ID);
                    //WriteToDevice(outputReportBuffer);
                }
                else
                {
                    Tracer.Error("FindTheHid() unsuccessful, skipped the ServoPositionSet command");
                }
            }
            catch (Exception ex)
            {
                Tracer.Error(ex);
                throw;
            }
        }
Beispiel #6
0
        // This will be called whenever the data is read from the board:
        private void DataReadCompleteHandler(object sender, AsyncInputReportArgs aira)
        {
            // do some quick sanity checking here to avoid exceptions:

            uint parkingSensorsCount = (uint)aira.InputBuffer[23];

            switch (parkingSensorsCount)
            {
            case 0:
            case 32:
            case 64:
                break;

            default:
                return;
            }

            switch (aira.InputBuffer[9])
            {
            case 0:
            case 1:
                break;

            default:
                return;
            }

            /*
             * Tracer.Trace("PM Command: Async data arrived. " + DateTime.Now);
             *
             *
             * StringBuilder byteValue = new StringBuilder();
             * byteValue.Append("DataReadCompleteHandler(): Input Report Data: ");
             *
             * for (int count = 0; count <= aira.InputBuffer.Length - 1; count++)
             * {
             *  //  Display bytes as 2-character Hex strings.
             *  byteValue.AppendFormat("{0:X02} ", aira.InputBuffer[count]);
             * }
             * Tracer.Trace(byteValue.ToString());
             */

            int servo1target = intFromBuffer(aira.InputBuffer, 1);
            int servo2target = intFromBuffer(aira.InputBuffer, 3);

            int ping1value = intFromBuffer(aira.InputBuffer, 5);
            int ping2value = intFromBuffer(aira.InputBuffer, 7);

            SensorsState sensState = new SensorsState();

            bool fromPingScanStop = aira.InputBuffer[9] > 0;

            // infrared distance sensors:
            sensState.irbE1 = aira.InputBuffer[10];
            sensState.irbE2 = aira.InputBuffer[11];
            sensState.irbE3 = aira.InputBuffer[12];
            sensState.irbE4 = aira.InputBuffer[13];

            sensState.irbO1 = aira.InputBuffer[14];
            sensState.irbO2 = aira.InputBuffer[15];
            sensState.irbO3 = aira.InputBuffer[16];
            sensState.irbO4 = aira.InputBuffer[17];

            sensState.compassHeading = (((uint)aira.InputBuffer[18] << 8) + (uint)aira.InputBuffer[19]) / 10.0d;

            sensState.accelX = ProximityBoard.toAccel(aira.InputBuffer[20]);
            sensState.accelY = ProximityBoard.toAccel(aira.InputBuffer[21]);
            sensState.accelZ = ProximityBoard.toAccel(aira.InputBuffer[22]);

            // ultrasound car parking sensors - bytes 23 to 31 (only first 4 bytes used, next 4 are reserved for 8-sensor device):
            sensState.parkingSensorsCount = parkingSensorsCount;          // 32 or 0 for invalid
            for (int i = 0; i < parkingSensorsCount / 8; i++)
            {
                sensState.parkingSensors[i] = aira.InputBuffer[24 + i];
            }
            sensState.mapParkingSensorsData();

            sensState.mapAnalogData(0, aira.InputBuffer[28], aira.InputBuffer[29]);    // LSB, MSB for AN0 (pin 2 on PIC 4550)

            // calibration for POT data (pin 2 RA0/AN0 on PIC4550):
            // 0v = 0
            // 1v = 220
            // 2v = 415
            // 3v = 630
            // 4v = 835
            // 4.88v = 1023

            AsyncInputFrameArgs args = new AsyncInputFrameArgs(servo1target, servo2target, ping1value, ping2value, fromPingScanStop, sensState);

            OnDataFrameComplete(args);

            if (inDataContinuousMode)
            {
                // initiate next read:
                ReadFromDevice(new EventHandler <AsyncInputReportArgs>(DataReadCompleteHandler), INPUT_CONT_REPORT_ID);
            }
        }
        private void ResetSweepCollector()
        {
            board = new ProximityBoard();

            inTestSamples = TEST_SAMPLES_COUNT;
        }