/// <summary> /// Read a motor status /// </summary> /// <param name="port">The Motor port to use, can be MotorLeft or MotorRight</param> /// <returns>Returns MotorStatus containing the status of the motor</returns> public MotorStatus GetMotorStatus(MotorPort port) { MotorStatus motorStatus = new MotorStatus(); SpiMessageType message_type = (port == MotorPort.MotorRight) ? SpiMessageType.GetMotorStatusRight : SpiMessageType.GetMotorStatusLeft; byte[] outArray = { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; var reply = SpiTransferArray(outArray); if (reply[3] == SpiCorrectDataReturned) { motorStatus.Speed = reply[5]; if ((motorStatus.Speed & 0x80) > 0) { motorStatus.Speed = -motorStatus.Speed; } motorStatus.Encoder = (int)(BinaryPrimitives.ReadInt32BigEndian(new Span <byte>(reply).Slice(6)) / MotorTicksPerDegree); motorStatus.Dps = ((reply[10] << 8) | reply[11]); if ((motorStatus.Dps & 0x8000) > 0) { motorStatus.Dps = motorStatus.Dps - 0x10000; } motorStatus.Flags = (MotorStatusFlags)reply[4]; } else { throw new IOException($"{nameof(GetMotorStatus)} error: no SPI response"); } return(motorStatus); }
/// <summary> /// Send a 32 - bit value over SPI /// </summary> /// <param name="MessageType">the SPI message type</param> /// <param name="Value">the value to be sent</param> public void SpiWrite32(SpiMessageType MessageType, int Value) { byte[] outArray = { SpiAddress, (byte)MessageType, (byte)((Value >> 24) & 0xFF), (byte)((Value >> 16) & 0xFF), (byte)((Value >> 8) & 0xFF), (byte)(Value & 0xFF) }; SpiTransferArray(outArray); }
/// <summary> /// Read a motor status /// </summary> /// <param name="port">The motor port (one at a time). PortA, PortB, PortC, or PortD.</param> /// <returns>MotorStatus containing the status of the motor</returns> public MotorStatus GetMotorStatus(byte port) { MotorStatus motorStatus = new MotorStatus(); SpiMessageType message_type = SpiMessageType.None; if (port == (byte)MotorPort.PortA) { message_type = SpiMessageType.GetMotorAStatus; } else if (port == (byte)MotorPort.PortB) { message_type = SpiMessageType.GetMotorBStatus; } else if (port == (byte)MotorPort.PortC) { message_type = SpiMessageType.GetMotorCStatus; } else if (port == (byte)MotorPort.PortD) { message_type = SpiMessageType.GetMotorDStatus; } else { throw new IOException( $"{nameof(GetMotorStatus)} error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D."); } byte[] outArray = { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; var reply = SpiTransferArray(outArray); if (reply[3] == 0xA5) { motorStatus.Speed = reply[5]; if ((motorStatus.Speed & 0x80) > 0) { motorStatus.Speed = -motorStatus.Speed; } motorStatus.Encoder = (reply[6] << 24) | (reply[7] << 16) | (reply[8] << 8) | reply[9]; // negative should be managed // if ((motorStatus.Encoder & 0x80000000) > 0) // motorStatus.Encoder = motorStatus.Encoder - 0x100000000; motorStatus.Dps = ((reply[10] << 8) | reply[11]); if ((motorStatus.Dps & 0x8000) > 0) { motorStatus.Dps = motorStatus.Dps - 0x10000; } motorStatus.Flags = (MotorStatusFlags)reply[4]; } else { throw new IOException($"{nameof(GetMotorStatus)} error: no SPI response"); } return(motorStatus); }
/// <summary> /// Read a 16 bit value over SPI /// </summary> /// <param name="MessageType">The SPI message type</param> /// <returns>Returns the value read</returns> public short SpiRead16(SpiMessageType MessageType) { byte[] outArray = { SpiAddress, (byte)MessageType, 0, 0, 0, 0, }; byte[] reply = SpiTransferArray(outArray); if (reply[3] == SpiCorrectDataReturned) { return(BinaryPrimitives.ReadInt16BigEndian(new Span <byte>(reply).Slice(4))); } throw new IOException($"{nameof(SpiRead16)} : no SPI response"); }
/// <summary> /// Read a 16 - bit value over SPI /// </summary> /// <param name="MessageType">the SPI message type</param> /// <returns>the vallue read</returns> public int SpiRead16(SpiMessageType MessageType) { int retVal = -1; byte[] outArray = { SpiAddress, (byte)MessageType, 0, 0, 0, 0, }; byte[] reply = SpiTransferArray(outArray); if (reply[3] == 0xA5) { retVal = (int)(reply[4] << 8) | reply[5]; } else { throw new IOException($"{nameof(SpiRead16)} : no SPI response"); } return(retVal); }
/// <summary> /// Send a 16 bit value over SPI /// </summary> /// <param name="MessageType">The SPI message type</param> /// <param name="Value">The value to be sent</param> public void SpiWrite16(SpiMessageType MessageType, short Value) { byte[] outArray = { SpiAddress, (byte)MessageType, (byte)((Value >> 8) & 0xFF), (byte)Value }; SpiTransferArray(outArray); }
/// <summary> /// Send a 8 bit value over SPI /// </summary> /// <param name="MessageType">The SPI message type</param> /// <param name="Value">The value to be sent</param> public void SpiWrite8(SpiMessageType MessageType, byte Value) { byte[] outArray = { SpiAddress, (byte)MessageType, Value }; SpiTransferArray(outArray); }
/// <summary> /// Read a 8 bit value over SPI /// </summary> /// <param name="MessageType">The SPI message type</param> /// <returns>Returns the value read</returns> public byte SpiRead8(SpiMessageType MessageType) { byte[] outArray = { SpiAddress, (byte)MessageType, 0, 0, 0 }; byte[] reply = SpiTransferArray(outArray); return((reply[3] == SpiCorrectDataReturned) ? reply[4] : throw new IOException($"{nameof(SpiRead16)} : no SPI response")); }
/// <summary> /// Get the sensor data from a specific port /// The following sensor types each return a single value: /// NONE----------------------- 0 /// TOUCH---------------------- 0 or 1(released or pressed) /// NXT_TOUCH------------------ 0 or 1(released or pressed) /// EV3_TOUCH------------------ 0 or 1(released or pressed) /// NXT_ULTRASONIC------------- distance in CM /// NXT_LIGHT_ON--------------- reflected light /// NXT_LIGHT_OFF-------------- ambient light /// NXT_COLOR_RED-------------- red reflected light /// NXT_COLOR_GREEN------------ green reflected light /// NXT_COLOR_BLUE------------- blue reflected light /// NXT_COLOR_OFF-------------- ambient light /// EV3_GYRO_ABS--------------- absolute rotation position in degrees /// EV3_GYRO_DPS--------------- rotation rate in degrees per second /// EV3_COLOR_REFLECTED-------- red reflected light /// EV3_COLOR_AMBIENT---------- ambient light /// EV3_COLOR_COLOR------------ detected color /// EV3_ULTRASONIC_CM---------- distance in CM /// EV3_ULTRASONIC_INCHES------ distance in inches /// EV3_ULTRASONIC_LISTEN------ 0 or 1(no other ultrasonic sensors or another ultrasonic sensor detected) /// EV3_INFRARED_PROXIMITY----- distance 0 - 100 % /// The following sensor types each return a list of values /// CUSTOM--------------------- Pin 1 ADC(5v scale from 0 to 4095), Pin 6 ADC(3.3v scale from 0 to 4095), Pin 5 digital, Pin 6 digital /// I2C------------------------ the I2C bytes read /// NXT_COLOR_FULL------------- detected color, red light reflected, green light reflected, blue light reflected, ambient light /// EV3_GYRO_ABS_DPS----------- absolute rotation position in degrees, rotation rate in degrees per second /// EV3_COLOR_RAW_REFLECTED---- red reflected light, unknown value(maybe a raw ambient value ?) /// EV3_COLOR_COLOR_COMPONENTS- red reflected light, green reflected light, blue reflected light, unknown value(maybe a raw value ?) /// EV3_INFRARED_SEEK---------- a list for each of the four channels.For each channel heading(-25 to 25), distance(-128 or 0 to 100) /// EV3_INFRARED_REMOTE-------- a list for each of the four channels.For each channel red up, red down, blue up, blue down, boadcast /// </summary> /// <param name="port">The sensor port(one at a time). Port 1, 2, 3, or 4</param> /// <returns>Returns the value(s) for the specified sensor.</returns> public byte[] GetSensor(byte port) { string ioErrorMessage = $"{nameof(GetSensor)} error: No SPI response"; string sensorErrorInvalidData = $"{nameof(GetSensor)} error: Invalid sensor data"; byte port_index = 0; SpiMessageType message_type = SpiMessageType.None; if (port == (byte)SensorPort.Port1) { message_type = SpiMessageType.GetSensor1; port_index = 0; } else if (port == (byte)SensorPort.Port2) { message_type = SpiMessageType.GetSensor2; port_index = 1; } else if (port == (byte)SensorPort.Port3) { message_type = SpiMessageType.GetSensor3; port_index = 2; } else if (port == (byte)SensorPort.Port4) { message_type = SpiMessageType.GetSensor4; port_index = 3; } else { throw new IOException( $"{nameof(GetSensor)} error. Must be one sensor port at a time. PORT_1, PORT_2, PORT_3, or PORT_4."); } List <byte> outArray = new List <byte>(); byte[] reply; if (_sensorType[port_index] == Models.SensorType.Custom) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { return(new byte[] { (byte)(((reply[8] & 0x0F) << 8) | reply[9]), (byte)(((reply[8] >> 4) & 0x0F) | (reply[7] << 4)), (byte)(reply[6] & 0x01), (byte)((reply[6] >> 1) & 0x01) }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if (_sensorType[port_index] == Models.SensorType.I2C) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0 }); for (int b = 0; b < _i2cInBytes[port_index]; b++) { outArray.Add(0); } reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData) && ((reply.Length - 6) == _i2cInBytes[port_index])) { List <byte> values = new List <byte>(); for (int b = 6; b < _i2cInBytes[port_index]; b++) { values.Add(reply[b]); } return(values.ToArray()); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if ((_sensorType[port_index] == Models.SensorType.Touch) || (_sensorType[port_index] == Models.SensorType.NXTTouch) || (_sensorType[port_index] == Models.SensorType.EV3Touch) || (_sensorType[port_index] == Models.SensorType.NXTUltrasonic) || (_sensorType[port_index] == Models.SensorType.EV3ColorReflected) || (_sensorType[port_index] == Models.SensorType.EV3ColorAmbient) || (_sensorType[port_index] == Models.SensorType.EV3ColorColor) || (_sensorType[port_index] == Models.SensorType.EV3UltrasonicListen) || (_sensorType[port_index] == Models.SensorType.EV3InfraredProximity)) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if (((reply[4] == (int)_sensorType[port_index]) || ((_sensorType[port_index] == Models.SensorType.Touch) && ((reply[4] == (int)Models.SensorType.NXTTouch) || (reply[4] == (int)Models.SensorType.EV3Touch)))) && (reply[5] == (int)SensorState.ValidData)) { return(new byte[] { reply[6] }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if (_sensorType[port_index] == Models.SensorType.NXTColorFull) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { return(new byte[] { reply[6], (byte)((reply[7] << 2) | ((reply[11] >> 6) & 0x03)), (byte)((reply[8] << 2) | ((reply[11] >> 4) & 0x03)), (byte)((reply[9] << 2) | ((reply[11] >> 2) & 0x03)), (byte)((reply[10] << 2) | (reply[11] & 0x03)) }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if ((_sensorType[port_index] == SensorType.NXTLightOn) || (_sensorType[port_index] == SensorType.NXTLightOff) || (_sensorType[port_index] == SensorType.NXTColorRed) || (_sensorType[port_index] == SensorType.NXTColorGreen) || (_sensorType[port_index] == SensorType.NXTColorBlue) || (_sensorType[port_index] == SensorType.NXTColorOff) || (_sensorType[port_index] == SensorType.EV3GyroAbs) || (_sensorType[port_index] == SensorType.EV3GyroDps) || (_sensorType[port_index] == SensorType.EV3UltrasonicCentimeter) || (_sensorType[port_index] == SensorType.EV3UltrasonicInches)) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { var value = (int)((reply[6] << 8) | reply[7]); if (((_sensorType[port_index] == SensorType.EV3GyroAbs) || (_sensorType[port_index] == SensorType.EV3GyroDps)) && ((value & 0x8000) > 0)) { value = value - 0x10000; } else if ((_sensorType[port_index] == SensorType.EV3UltrasonicCentimeter) || (_sensorType[port_index] == SensorType.EV3UltrasonicInches)) { value = value / 10; } // convert back the value to a byte array return(new byte[] { (byte)((value >> 8) & 0xFF), (byte)(value & 0xFF) }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if ((_sensorType[port_index] == SensorType.EV3ColorRawReflected) || (_sensorType[port_index] == SensorType.EV3GyroAbsDps)) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { ushort[] results = new ushort[] { (ushort)((reply[6] << 8) | reply[7]), (ushort)((reply[8] << 8) | reply[9]) }; if (_sensorType[port_index] == SensorType.EV3GyroAbsDps) { for (int r = 0; r < results.Length; r++) { if (results[r] >= 0x8000) { results[r] = (ushort)(results[r] - 0x10000); } } } // convert back the value to a byte array // we know the length is 2 return(new byte[] { (byte)((results[1] >> 8) & 0xFF), (byte)(results[1] & 0xFF), (byte)((results[0] >> 8) & 0xFF), (byte)(results[0] & 0xFF) }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if ((_sensorType[port_index] == SensorType.EV3ColorColorComponents)) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) || (reply[5] == (int)SensorState.ValidData)) { return(new byte[] { reply[6], reply[7], reply[8], reply[9], reply[10], reply[11], reply[12], reply[13] }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if (_sensorType[port_index] == SensorType.EV3InfraredSeek) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { return(new byte[] { (reply[6]), (reply[7]), (reply[8]), (reply[9]), (reply[10]), (reply[11]), (reply[12]), (reply[13]) }); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } else if (_sensorType[port_index] == Models.SensorType.EV3InfraredRemote) { outArray.AddRange(new byte[] { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 }); reply = SpiTransferArray(outArray.ToArray()); if (reply[3] == 0xA5) { if ((reply[4] == (int)_sensorType[port_index]) && (reply[5] == (int)SensorState.ValidData)) { byte[] results = new byte[] { 0, 0, 0, 0 }; for (int r = 0; r < results.Length; r++) { var value = reply[6 + r]; if (value == 1) { results[r] = 0b10000; } else if (value == 2) { results[r] = 0b01000; } else if (value == 3) { results[r] = 0b00100; } else if (value == 4) { results[r] = 0b00010; } else if (value == 5) { results[r] = 0b10100; } else if (value == 6) { results[r] = 0b10010; } else if (value == 7) { results[r] = 0b01100; } else if (value == 8) { results[r] = 0b01010; } else if (value == 9) { results[r] = 0b00001; } else if (value == 10) { results[r] = 0b11000; } else if (value == 11) { results[r] = 0b00110; } else { results[r] = 0b00000; } } return(results); } else { throw new IOException(sensorErrorInvalidData); } } else { throw new IOException(ioErrorMessage); } } throw new IOException($"{nameof(GetSensor)} error: Sensor not configured or not supported."); }