public static short[] CompassData()
{
Log.Module = Module;
short[] _out_value = new short[] { 0, 0, 0 };
byte[] data = new byte[2];
if (Server.I2C.Connected == true)
{
byte i2c_address_write = (byte)(i2c_address >> 1);
RoBoIO.i2c0master_StartN(i2c_address_write, (byte)RoBoIO.I2C_WRITE, 2); //write 2 byte
RoBoIO.i2c0master_WriteN(0x02); //mode register
RoBoIO.i2c0master_WriteN(0x00); //continue-measureture mode
Log.WriteLineMessage("Read 3-axis values of Compass...");
RoBoIO.delay_ms(100);
RoBoIO.i2c0master_StartN(i2c_address_write, (byte)RoBoIO.I2C_WRITE, 1);
RoBoIO.i2c0master_SetRestartN((byte)RoBoIO.I2C_READ, 6);
RoBoIO.i2c0master_WriteN(0x03); //Read from data register (Address : 0x03)
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
_out_value[0] = System.BitConverter.ToInt16(data, 0);
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
_out_value[1] = System.BitConverter.ToInt16(data, 0);
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
_out_value[2] = System.BitConverter.ToInt16(data, 0);
}
return(_out_value);
}
public static short[] AcceleroData()
{
Log.Module = Module;
short[] _out_value = new short[] { 0, 0, 0 };
byte[] data = new byte[2];
if (Server.I2C.Connected == true)
{
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 2); //write 2 byte
RoBoIO.i2c0master_WriteN(0x2d); //Power_Control register
RoBoIO.i2c0master_WriteN(0x28); //link and measure mode
Log.WriteLineMessage("Read 3-axis values of Accelerometer...");
// RoBoIO.delay_ms(100);
RoBoIO.delay_ms(50);
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 2); //write 2 byte
RoBoIO.i2c0master_WriteN(0x31); //Data_Format register
RoBoIO.i2c0master_WriteN(0x08); //Full_Resolution
// RoBoIO.delay_ms(100);
RoBoIO.delay_ms(50);
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 2); //write 2 byte
RoBoIO.i2c0master_WriteN(0x38); //FIFO_Control register
RoBoIO.i2c0master_WriteN(0x00); //bypass mode
// RoBoIO.delay_ms(100);
RoBoIO.delay_ms(50);
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 1);
RoBoIO.i2c0master_SetRestartN((byte)RoBoIO.I2C_READ, 6);
RoBoIO.i2c0master_WriteN(0x32); //Read from X register (Address : 0x32)
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
_out_value[0] = System.BitConverter.ToInt16(data, 0);
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
_out_value[1] = System.BitConverter.ToInt16(data, 0);
data[0] = (byte)RoBoIO.i2c0master_ReadN(); //X LSB
data[1] = (byte)RoBoIO.i2c0master_ReadN(); //X MSB
_out_value[2] = System.BitConverter.ToInt16(data, 0);
}
return(_out_value);
}
public static short[] GyroData()
{
Log.Module = Module;
short[] _out_value = new short[] { 0, 0, 0, 0, 0, 0, 0, 0 };
if (Server.I2C.Connected == true)
{
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 2); //AS pin is high
RoBoIO.i2c0master_WriteN(0x03); //cycle time register
RoBoIO.i2c0master_WriteN(0x01); //convert time
Log.WriteLineMessage("Read 3-axis values of Gyro and chip temperature...");
for (int i = 0; i < 8; i++)
{
high = (byte)((0xF0 & (0x01 << i)) >> 4); //CH5 ~ CH8
low = (byte)((0x0F & (0x01 << i)) << 4); //CH1 ~ CH4
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 3); //write 3 bytes
RoBoIO.i2c0master_WriteN(0x02); //configuration register
RoBoIO.i2c0master_WriteN(high);
RoBoIO.i2c0master_WriteN((byte)(low + 0x0C)); //0x0c : FLTR = 1,ALERT/EN = 1
RoBoIO.delay_ms(10);
RoBoIO.i2c0master_StartN(i2c_address, (byte)RoBoIO.I2C_WRITE, 1);
RoBoIO.i2c0master_SetRestartN((byte)RoBoIO.I2C_READ, 2);
RoBoIO.i2c0master_WriteN(0x00); //Read data form Conversion Result Register
//Data : 12bits
d1 = (byte)RoBoIO.i2c0master_ReadN();
d2 = (byte)RoBoIO.i2c0master_ReadN();
_out_value[((d1 & 0x70) >> 4)] = ((short)((d1 & 0x0f) * 256 + d2));
}
}
return(_out_value);
}
