public void spi_write_32(SPI_MESSAGE_TYPE MessageType, int Value)
{
// Send a 32 - bit value over SPI
// Keyword arguments:
// MessageType-- the SPI message type
// Value-- the value to be sent
byte[] outArray = { SPI_Address, (byte)MessageType, (byte)((Value >> 24) & 0xFF), (byte)((Value >> 16) & 0xFF), (byte)((Value >> 8) & 0xFF), (byte)(Value & 0xFF) };
spi_transfer_array(outArray);
}
public int spi_read_16(SPI_MESSAGE_TYPE MessageType)
{
/* Read a 16 - bit value over SPI
* Keyword arguments:
* MessageType-- the SPI message type
* Returns touple:
* value, error
*/
int retVal = -1;
byte[] outArray = { SPI_Address, (byte)MessageType, 0, 0, 0, 0, };
byte[] reply = spi_transfer_array(outArray);
if (reply[3] == 0xA5)
{
retVal = (int)(reply[4] << 8) | reply[5];
}
else
{
throw new IOError("No SPI response");
}
return(retVal);
}
public MotorStatus get_motor_status(byte port)
{
//Read a motor status
//Keyword arguments:
//port -- The motor port (one at a time). PORT_A, PORT_B, PORT_C, or PORT_D.
//Returns a list:
// flags -- 8-bits of bit-flags that indicate motor status:
// bit 0 -- LOW_VOLTAGE_FLOAT - The motors are automatically disabled because the battery voltage is too low
// bit 1 -- OVERLOADED - The motors aren't close to the target (applies to position control and dps speed control).
// power -- the raw PWM power in percent (-100 to 100)
// encoder -- The encoder position
// dps -- The current speed in Degrees Per Second
MotorStatus motorStatus = new MotorStatus();
SPI_MESSAGE_TYPE message_type = SPI_MESSAGE_TYPE.NONE;
if (port == (byte)MOTOR_PORT.PORT_A)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_A_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_B)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_B_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_C)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_C_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_D)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_D_STATUS;
}
else
{
throw new IOError("get_motor_status error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D.");
}
byte[] outArray = { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
var reply = spi_transfer_array(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 IOError("No SPI response");
}
return(motorStatus);
}
public byte[] get_sensor(byte port)
{
// Read a sensor value
//Keyword arguments:
//port-- The sensor port(one at a time).PORT_1, PORT_2, PORT_3, or PORT_4.
//Returns the value(s) for the specified sensor.
// 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
byte port_index = 0;
SPI_MESSAGE_TYPE message_type = SPI_MESSAGE_TYPE.NONE;
if (port == (byte)SENSOR_PORT.PORT_1)
{
message_type = SPI_MESSAGE_TYPE.GET_SENSOR_1;
port_index = 0;
}
else if (port == (byte)SENSOR_PORT.PORT_2)
{
message_type = SPI_MESSAGE_TYPE.GET_SENSOR_2;
port_index = 1;
}
else if (port == (byte)SENSOR_PORT.PORT_3)
{
message_type = SPI_MESSAGE_TYPE.GET_SENSOR_3;
port_index = 2;
}
else if (port == (byte)SENSOR_PORT.PORT_4)
{
message_type = SPI_MESSAGE_TYPE.GET_SENSOR_4;
port_index = 3;
}
else
{
throw new IOError("get_sensor 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] == SENSOR_TYPE.CUSTOM)
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
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 SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if (SensorType[port_index] == SENSOR_TYPE.I2C)
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0 });
for (int b = 0; b < I2CInBytes[port_index]; b++)
{
outArray.Add(0);
}
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA) && ((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 SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if ((SensorType[port_index] == SENSOR_TYPE.TOUCH) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_TOUCH) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_TOUCH) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_ULTRASONIC) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_COLOR_REFLECTED) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_COLOR_AMBIENT) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_COLOR_COLOR) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_ULTRASONIC_LISTEN) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_INFRARED_PROXIMITY))
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
// if (reply[4] == self.SensorType[port_index] or (self.SensorType[port_index] == self.SENSOR_TYPE.TOUCH and (reply[4] == self.SENSOR_TYPE.NXT_TOUCH or reply[4] == self.SENSOR_TYPE.EV3_TOUCH))) and reply[5] == self.SENSOR_STATE.VALID_DATA
if (((reply[4] == (int)SensorType[port_index]) || ((SensorType[port_index] == SENSOR_TYPE.TOUCH) && ((reply[4] == (int)SENSOR_TYPE.NXT_TOUCH) ||
(reply[4] == (int)SENSOR_TYPE.EV3_TOUCH)))) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
return new byte[] { reply[6] }
}
;
else
{
throw new SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if (SensorType[port_index] == SENSOR_TYPE.NXT_COLOR_FULL)
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
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 SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if ((SensorType[port_index] == SENSOR_TYPE.NXT_LIGHT_ON) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_LIGHT_OFF) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_COLOR_RED) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_COLOR_GREEN) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_COLOR_BLUE) ||
(SensorType[port_index] == SENSOR_TYPE.NXT_COLOR_OFF) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_ABS) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_DPS) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_ULTRASONIC_CM) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_ULTRASONIC_INCHES))
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
var value = (int)((reply[6] << 8) | reply[7]);
if (((SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_ABS) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_DPS)) &&
((value & 0x8000) > 0))
{
value = value - 0x10000;
}
else if ((SensorType[port_index] == SENSOR_TYPE.EV3_ULTRASONIC_CM) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_ULTRASONIC_INCHES))
{
value = value / 10;
}
//convert back the value to a byte array
return(new byte[] { (byte)((value >> 8) & 0xFF), (byte)(value & 0xFF) });
}
else
{
throw new SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if ((SensorType[port_index] == SENSOR_TYPE.EV3_COLOR_RAW_REFLECTED) ||
(SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_ABS_DPS))
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
ushort[] results = new ushort[] { (ushort)((reply[6] << 8) | reply[7]), (ushort)((reply[8] << 8) | reply[9]) };
if (SensorType[port_index] == SENSOR_TYPE.EV3_GYRO_ABS_DPS)
{
//TODO: check it is necessary to do the convertion and not just pass the byte array and do it later
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 SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if ((SensorType[port_index] == SENSOR_TYPE.EV3_COLOR_COLOR_COMPONENTS))
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) || (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
return new byte[] { reply[6], reply[7], reply[8], reply[9], reply[10], reply[11], reply[12], reply[13] }
}
;
else
{
throw new SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if (SensorType[port_index] == SENSOR_TYPE.EV3_INFRARED_SEEK)
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{//create a simple table of bytes, not a byte byte arrays like in Python
byte[] results = new byte[] { (reply[6]), (reply[7]), (reply[8]), (reply[9]), (reply[10]), (reply[11]), (reply[12]), (reply[13]) };
//No convertion done at this level, convertion should be done when receiving the results
//for c in range(len(results)):
// for v in range(len(results[c])):
// if results[c][v] >= 0x80:
// results[c][v] = results[c][v] - 0x100
return(results);
}
else
{
throw new SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
else if (SensorType[port_index] == SENSOR_TYPE.EV3_INFRARED_REMOTE)
{
outArray.AddRange(new byte[] { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0 });
reply = spi_transfer_array(outArray.ToArray());
if (reply[3] == 0xA5)
{
if ((reply[4] == (int)SensorType[port_index]) && (reply[5] == (int)SENSOR_STATE.VALID_DATA))
{
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 SensorError("get_sensor error: Invalid sensor data");
}
}
else
{
throw new IOError("get_sensor error: No SPI response");
}
}
throw new IOError("get_sensor error: Sensor not configured or not supported.");
}
