/// <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.");
}