Пример #1
0
        /// <summary>
        /// update the type of sensors. Needed to be able to change mode
        /// don't call directly this function, it will brak the running thread. Call it only thru the
        /// SetupSensros function.
        /// </summary>
        /// <returns></returns>
        private async Task <bool> BrickPiSetupSensors(int idxArduino)
        {
            DataArray dataArray = new DataArray();
            bool      retval    = true;
            int       retry     = 0;

            dataArray.Bit_Offset                  = 0;
            dataArray.myArray[BYTE_MSG_TYPE]      = MSG_TYPE_SENSOR_TYPE;
            dataArray.myArray[BYTE_SENSOR_1_TYPE] = (byte)brickPi.Sensor[PORT_1 + idxArduino * 2].Type;
            dataArray.myArray[BYTE_SENSOR_2_TYPE] = (byte)brickPi.Sensor[PORT_2 + idxArduino * 2].Type;
            bool iscolor = false;

            for (int idxPort = 0; idxPort < 2; idxPort++)
            {
                int port = idxArduino * 2 + idxPort;
                //check if there is a sensor Light. If yes, then we'll need to wait super long to have it setup
                //othewise, quite fast
                if ((brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M0) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M1) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M2) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M3) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M4) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M5) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.COLOR_BLUE) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.COLOR_FULL) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.COLOR_GREEN) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.COLOR_NONE) ||
                    (brickPi.Sensor[port].Type == BrickSensorType.COLOR_RED))
                {
                    iscolor = true;
                }
                if (dataArray.myArray[BYTE_SENSOR_1_TYPE + idxPort] == (byte)BrickSensorType.ULTRASONIC_CONT)
                {
                    dataArray.myArray[BYTE_SENSOR_1_TYPE + idxPort] = (byte)BrickSensorType.I2C;
                    brickPi.I2C[port].Speed   = US_I2C_SPEED;
                    brickPi.I2C[port].Devices = 1;
                    brickPi.Sensor[port].Settings[US_I2C_IDX]  = BIT_I2C_MID | BIT_I2C_SAME;
                    brickPi.I2C[port].Address[US_I2C_IDX]      = LEGO_US_I2C_ADDR;
                    brickPi.I2C[port].Write[US_I2C_IDX]        = 1;
                    brickPi.I2C[port].Read[US_I2C_IDX]         = 1;
                    brickPi.I2C[port].Out[US_I2C_IDX].InOut[0] = LEGO_US_I2C_DATA_REG;
                }
                if ((dataArray.myArray[BYTE_SENSOR_1_TYPE + idxPort] == (byte)BrickSensorType.I2C) || (dataArray.myArray[BYTE_SENSOR_1_TYPE + idxPort] == (byte)BrickSensorType.I2C_9V))
                {
                    dataArray.AddBits(3, 0, 8, brickPi.I2C[port].Speed);
                    if (brickPi.I2C[port].Devices > 8)
                    {
                        brickPi.I2C[port].Devices = 8;
                    }

                    if (brickPi.I2C[port].Devices == 0)
                    {
                        brickPi.I2C[port].Devices = 1;
                    }

                    dataArray.AddBits(3, 0, 3, (brickPi.I2C[port].Devices - 1));

                    for (int device = 0; device < brickPi.I2C[port].Devices; device++)
                    {
                        dataArray.AddBits(3, 0, 7, (brickPi.I2C[port].Address[device] >> 1));
                        dataArray.AddBits(3, 0, 2, brickPi.Sensor[port].Settings[device]);
                        if ((brickPi.Sensor[port].Settings[device] & BIT_I2C_SAME) == BIT_I2C_SAME)
                        {
                            dataArray.AddBits(3, 0, 4, brickPi.I2C[port].Write[device]);
                            dataArray.AddBits(3, 0, 4, brickPi.I2C[port].Read[device]);

                            for (int out_byte = 0; out_byte < brickPi.I2C[port].Write[device]; out_byte++)
                            {
                                dataArray.AddBits(3, 0, 8, brickPi.I2C[port].Out[device].InOut[out_byte]);
                            }
                        }
                    }
                }
            }
            while (retry < 2)
            {
                int tx_bytes = (((dataArray.Bit_Offset + 7) / 8) + 3); //#eq to UART_TX_BYTES
                BrickPiTx(brickPi.Address[idxArduino], tx_bytes, dataArray.myArray);
                //# Timeout set to 5 seconds to setup EV3 sensors successfully
                int timeout = 100;
                if (iscolor)
                {
                    timeout = 1000;
                    Debug.WriteLine("initializing color sensor");
                }
                //use a small delay for cancellation if issue, for setup, should be fast
                byte[] InArray = await BrickPiRx(timeout);

                if (InArray == null)
                {
                    retval = false;
                }
                else
                if (((InArray[BYTE_MSG_TYPE] == MSG_TYPE_SENSOR_TYPE) && (InArray.Length == 1)))
                {
                    return(true);
                }
                retry++;
                Debug.WriteLine("Trying again to setup sensors");
            }
            return(retval);
        }
Пример #2
0
        /// <summary>
        /// Main function running in the main thread, checking all the time the status of sensors
        /// </summary>
        /// <returns></returns>
        private async Task <bool> BrickPiUpdateValues()
        {
            //If setup is needed, then first setup the sensors
            await SetupSensors();

            //if need to change the timeout of the brick
            if (needTimeout)
            {
                BrickPiSetTimeout().Wait();
                needTimeout = false;
            }


            DataArray dataArray = new DataArray();
            int       Retried   = 0;

            bool ret        = false;
            int  idxArduino = 0;

            while (idxArduino < 2)
            {
                if (!ret)
                {
                    Retried = 0;
                }
                //Fill the header of buffer for communication
                // BYTE_MSG_TYPE, position 0, Message to send
                for (int ii = 0; ii < dataArray.myArray.Length; ii++)
                {
                    dataArray.myArray[ii] = 0;
                }
                dataArray.myArray[BYTE_MSG_TYPE] = MSG_TYPE_VALUES;
                dataArray.Bit_Offset             = 0;
                // This second part is specific to motors encoders.
                for (int idxPort = 0; idxPort < 2; idxPort++)
                {
                    int port = (idxArduino * 2) + idxPort;
                    if (BrickPi.Motor[port].EncoderOffset != 0)
                    {
                        int Temp_Value = BrickPi.Motor[port].EncoderOffset;
                        dataArray.AddBits(1, 0, 1, 1);
                        int Temp_ENC_DIR = 0;
                        if (Temp_Value < 0)
                        {
                            Temp_ENC_DIR = 1;
                            Temp_Value  *= -1;
                        }
                        byte Temp_BitsNeeded = (byte)(dataArray.BitsNeeded(Temp_Value) + 1);
                        dataArray.AddBits(1, 0, 5, Temp_BitsNeeded);
                        Temp_Value *= 2;
                        Temp_Value |= Temp_ENC_DIR;
                        dataArray.AddBits(1, 0, Temp_BitsNeeded, Temp_Value);
                    }
                    else
                    {
                        dataArray.AddBits(1, 0, 1, 0);
                    }
                }
                // This third part is specific to motors speed and direction
                for (int idxPort = 0; idxPort < 2; idxPort++)
                {
                    int port  = (idxArduino * 2) + idxPort;
                    int speed = BrickPi.Motor[port].Speed;
                    int direc = 0;
                    if (speed < 0)
                    {
                        direc  = 1;
                        speed *= -1;
                    }
                    if (speed > 255)
                    {
                        speed = 255;
                    }
                    dataArray.AddBits(1, 0, 10, ((((speed & 0xFF) << 2) | (direc << 1) | (BrickPi.Motor[port].Enable & 0x01)) & 0x3FF));
                }
                // This part is specific to I2C sensors
                for (int idxPort = 0; idxPort < 2; idxPort++)
                {
                    int port = (idxArduino * 2) + idxPort;
                    if ((BrickPi.Sensor[port].Type == BrickSensorType.I2C) ||
                        (BrickPi.Sensor[port].Type == BrickSensorType.I2C_9V) ||
                        (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT))
                    {
                        for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
                        {
                            if ((BrickPi.Sensor[port].Settings[device] & BIT_I2C_SAME) != BIT_I2C_SAME)
                            {
                                dataArray.AddBits(1, 0, 4, BrickPi.I2C[port].Write[device]);
                                dataArray.AddBits(1, 0, 4, BrickPi.I2C[port].Read[device]);
                                for (int out_byte = 0; out_byte < BrickPi.I2C[port].Write[device]; out_byte++)
                                {
                                    dataArray.AddBits(1, 0, 8, BrickPi.I2C[port].Out[device].InOut[out_byte]);
                                }
                            }
                            // ITODO don't understadn why??? so letting it as it is:
                            device += 1;
                        }
                    }
                }
                byte[] InArray = null;
                try
                {
                    int tx_bytes = (((dataArray.Bit_Offset + 7) / 8) + 1); // #eq to UART_TX_BYTES
                    BrickPiTx(BrickPi.Address[idxArduino], tx_bytes, dataArray.myArray);
                    //wait for answer
                    // IMPORTANT: in theory, waiting time if 7.5 ms but it does create problems
                    // so it is working fine with a 20 ms timeout.
                    InArray = await BrickPiRx(15); //theory 7.5 ms
                }
                catch (Exception ex)
                {
                    Debug.WriteLine(string.Format("Error reading/writing: {0}", ex.Message));
                    continue;
                }
                if (InArray == null)
                {
                    continue;
                }
                //Send the data

                // first part is about encoders
                BrickPi.Motor[(idxArduino * 2) + (int)BrickPortMotor.PORT_A].EncoderOffset = 0;
                BrickPi.Motor[(idxArduino * 2) + (int)BrickPortMotor.PORT_B].EncoderOffset = 0;
                byte[] OutArray;
                //check if we got the right answer to the question
                if (!CheckRetMessage(InArray, MSG_TYPE_VALUES, out OutArray))
                {
                    Debug.WriteLine(String.Format("BrickPi Error reading value in Updating values"));
                    if (Retried < 2)
                    {
                        ret      = true;
                        Retried += 1;
                        continue;
                    }
                }
                dataArray.myArray = OutArray;

                ret = false;
                dataArray.Bit_Offset = 0;
                // number of bits to be used for encoders
                List <byte> Temp_BitsUsed = new List <byte>();

                Temp_BitsUsed.Add((byte)dataArray.GetBits(1, 0, 5));
                Temp_BitsUsed.Add((byte)dataArray.GetBits(1, 0, 5));

                for (int idxPort = 0; idxPort < 2; idxPort++)
                {
                    long Temp_EncoderVal = dataArray.GetBits(1, 0, Temp_BitsUsed[idxPort]);
                    if ((Temp_EncoderVal & 0x01) == 1)
                    {
                        Temp_EncoderVal /= 2;
                        BrickPi.Motor[idxPort + idxArduino * 2].Encoder = (int)(Temp_EncoderVal) * -1;
                    }
                    else
                    {
                        BrickPi.Motor[idxPort + idxArduino * 2].Encoder = (int)(Temp_EncoderVal / 2);
                    }
                }
                // This is the part with sensors
                for (int idxPort = 0; idxPort < 2; idxPort++)
                {
                    int port = idxPort + (idxArduino * 2);
                    switch (BrickPi.Sensor[port].Type)
                    {
                    case BrickSensorType.SENSOR_RAW:
                        //this is 0 value, LIGHT_OFF is as well 0
                        //case BrickSensorType.LIGHT_OFF:
                        break;

                    case BrickSensorType.LIGHT_ON:
                        break;

                    case BrickSensorType.TOUCH:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 1);
                        break;

                    case BrickSensorType.ULTRASONIC_SS:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 8);
                        break;

                    case BrickSensorType.RCX_LIGHT:
                        break;

                    case BrickSensorType.COLOR_FULL:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 3);
                        BrickPi.Sensor[port].Array[INDEX_BLANK] = (int)dataArray.GetBits(1, 0, 10);
                        BrickPi.Sensor[port].Array[INDEX_RED]   = (int)dataArray.GetBits(1, 0, 10);
                        BrickPi.Sensor[port].Array[INDEX_GREEN] = (int)dataArray.GetBits(1, 0, 10);
                        BrickPi.Sensor[port].Array[INDEX_BLUE]  = (int)dataArray.GetBits(1, 0, 10);
                        break;

                    case BrickSensorType.ULTRASONIC_CONT:
                    case BrickSensorType.I2C:
                    case BrickSensorType.I2C_9V:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, BrickPi.I2C[port].Devices);
                        for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
                        {
                            if ((BrickPi.Sensor[port].Value & (0x01 << device)) != 0)
                            {
                                for (int in_byte = 0; in_byte < BrickPi.I2C[port].Read[device]; in_byte++)
                                {
                                    BrickPi.I2C[port].In[device].InOut[in_byte] = (int)dataArray.GetBits(1, 0, 8);
                                }
                            }
                        }
                        if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT)
                        {
                            if (((int)BrickPi.Sensor[port].Value & (0x01 << US_I2C_IDX)) != 0)
                            {
                                BrickPi.Sensor[port].Value = BrickPi.I2C[port].In[US_I2C_IDX].InOut[0];
                            }
                            else
                            {
                                BrickPi.Sensor[port].Value = -1;
                            }
                        }

                        break;

                    case BrickSensorType.EV3_INFRARED_M2:
                    case BrickSensorType.EV3_GYRO_M3:
                    case BrickSensorType.EV3_COLOR_M3:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
                        break;

                    case BrickSensorType.EV3_US_M0:
                    case BrickSensorType.EV3_US_M1:
                    case BrickSensorType.EV3_US_M2:
                    case BrickSensorType.EV3_US_M3:
                    case BrickSensorType.EV3_US_M4:
                    case BrickSensorType.EV3_US_M5:
                    case BrickSensorType.EV3_US_M6:
                    case BrickSensorType.EV3_COLOR_M0:
                    case BrickSensorType.EV3_COLOR_M1:
                    case BrickSensorType.EV3_COLOR_M2:
                    case BrickSensorType.EV3_COLOR_M4:
                    case BrickSensorType.EV3_COLOR_M5:
                    case BrickSensorType.EV3_GYRO_M0:
                    case BrickSensorType.EV3_GYRO_M1:
                    case BrickSensorType.EV3_GYRO_M2:
                    case BrickSensorType.EV3_GYRO_M4:
                    case BrickSensorType.EV3_INFRARED_M0:
                    case BrickSensorType.EV3_INFRARED_M1:
                    case BrickSensorType.EV3_INFRARED_M3:
                    case BrickSensorType.EV3_INFRARED_M4:
                    case BrickSensorType.EV3_INFRARED_M5:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
                        //# EV3 Gyro Mode 0, Adjust sign
                        if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M0)
                        {
                            if (BrickPi.Sensor[port].Value >= 32767)            //# Negative number.  This seems to return a 2 byte number.
                            {
                                BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
                            }
                        }
                        //# EV3 Gyro Mode 1, Adjust sign
                        else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M1)
                        {
                            if (BrickPi.Sensor[port].Value >= 32767)     //		# Negative number.  This seems to return a 2 byte number.
                            {
                                BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
                            }
                        }
                        break;

                    case BrickSensorType.EV3_TOUCH_0:
                        BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
                        break;

                    case BrickSensorType.EV3_TOUCH_DEBOUNCE:
                    case BrickSensorType.TOUCH_DEBOUNCE:
                    case BrickSensorType.COLOR_RED:
                    case BrickSensorType.COLOR_GREEN:
                    case BrickSensorType.COLOR_BLUE:
                    case BrickSensorType.COLOR_NONE:
                    default:
                        BrickPi.Sensor[idxPort + (idxArduino * 2)].Value = (int)dataArray.GetBits(1, 0, 10);
                        break;
                    }
                    #region oldcode
                    //if (BrickPi.Sensor[port].Type == BrickSensorType.TOUCH)
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 1);
                    //else if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_SS)
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 8);
                    //else if (BrickPi.Sensor[port].Type == BrickSensorType.COLOR_FULL)
                    //{
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 3);
                    //    BrickPi.Sensor[port].Array[INDEX_BLANK] = (int)dataArray.GetBits(1, 0, 10);
                    //    BrickPi.Sensor[port].Array[INDEX_RED] = (int)dataArray.GetBits(1, 0, 10);
                    //    BrickPi.Sensor[port].Array[INDEX_GREEN] = (int)dataArray.GetBits(1, 0, 10);
                    //    BrickPi.Sensor[port].Array[INDEX_BLUE] = (int)dataArray.GetBits(1, 0, 10);
                    //}
                    //else if ((BrickPi.Sensor[port].Type == BrickSensorType.I2C) || (BrickPi.Sensor[port].Type == BrickSensorType.I2C_9V) || (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT))
                    //{
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, BrickPi.I2C[port].Devices);
                    //    for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
                    //    {
                    //        if ((BrickPi.Sensor[port].Value & (0x01 << device)) != 0)
                    //            for (int in_byte = 0; in_byte < BrickPi.I2C[port].Read[device]; in_byte++)
                    //                BrickPi.I2C[port].In[device].InOut[in_byte] = (int)dataArray.GetBits(1, 0, 8);
                    //    }
                    //}
                    //else if ((BrickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M3) || (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M3))
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
                    //else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_INFRARED_M2)
                    //{
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
                    //    if (BrickPi.Sensor[port].Value < 0)
                    //        Debug.WriteLine("IR SENSOR RETURNED ERROR");
                    //}
                    //else if ((BrickPi.Sensor[port].Type >= BrickSensorType.EV3_US_M0) && (BrickPi.Sensor[port].Type <= BrickSensorType.EV3_INFRARED_M5 + 1))
                    //    BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
                    //else // #For all the light, color and raw sensors
                    //    BrickPi.Sensor[ii + (i * 2)].Value = (int)dataArray.GetBits(1, 0, 10);

                    //if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT)
                    //{
                    //    if (((int)BrickPi.Sensor[port].Value & (0x01 << US_I2C_IDX)) != 0)
                    //        BrickPi.Sensor[port].Value = BrickPi.I2C[port].In[US_I2C_IDX].InOut[0];
                    //    else
                    //        BrickPi.Sensor[port].Value = -1;
                    //}


                    //# EV3 Gyro Mode 0, Adjust sign
                    //if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M0)
                    //{
                    //    if (BrickPi.Sensor[port].Value >= 32767)		//# Negative number.  This seems to return a 2 byte number.
                    //        BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
                    //    //#else:					# Positive Number print str(gyro)
                    //    //#######################
                    //    //# EV3 Gyro Mode 1, Adjust sign
                    //}
                    //else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M1)
                    //{
                    //    //                # print "Gyro m1!"
                    //    if (BrickPi.Sensor[port].Value >= 32767) //		# Negative number.  This seems to return a 2 byte number.
                    //        BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
                    //    //				# else:					# Positive Number print str(gyro)
                    //}
                    //            # print BrickPi.SensorType[port]
                    #endregion
                }
                idxArduino++;
            }
            //if all went correctly, then ret should be false
            return(!ret);
        }