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);
}
// Method
//
public static bool Init()
{
if (!SPI.Connected)
{
SPI.Init();
}
Log.Module = Module;
if (!_connected)
{
if (RoBoIO.ad7918_InitializeMCH(RoBoIO.AD7918_USECHANNEL0 +
RoBoIO.AD7918_USECHANNEL1 +
RoBoIO.AD7918_USECHANNEL2 +
RoBoIO.AD7918_USECHANNEL3 +
RoBoIO.AD7918_USECHANNEL4 +
RoBoIO.AD7918_USECHANNEL5 +
RoBoIO.AD7918_USECHANNEL6 +
RoBoIO.AD7918_USECHANNEL7,
RoBoIO.AD7918MODE_RANGE_2VREF,
RoBoIO.AD7918MODE_CODING_1023))
{
_connected = true;
Log.WriteLineSucces("Opening: AD7918 lib");
return(true);
}
_connected = false;
Log.WriteLineFail("Opening: AD7918 lib");
Log.WriteLineError(string.Format("AD7918 lib fails to open ({0})", RoBoIO_DotNet.RoBoIO.roboio_GetErrMsg()));
return(false);
}
Log.WriteLineMessage("Opening: AD7918 lib...already open");
return(true);
}
public static string CPU()
{
string message = string.Empty;
switch (RoBoIO.io_CpuID())
{
case RoBoIO.CPU_UNSUPPORTED:
message = "CPU Unsupported";
break;
case RoBoIO.CPU_VORTEX86DX_1:
message = "CPU VORTEX86DX1";
break;
case RoBoIO.CPU_VORTEX86DX_2:
message = "CPU VORTEX86DX2";
break;
case RoBoIO.CPU_VORTEX86DX_3:
message = "CPU VORTEX86DX3";
break;
case RoBoIO.CPU_VORTEX86SX:
message = "CPU VORTEX86SX";
break;
default:
message = "CPU";
break;
}
return(message);
}
public static string Version()
{
string message = string.Empty;
switch (RoBoIO.roboio_GetRBVer())
{
case RoBoIO.RB_100:
message = "RB-100";
break;
case RoBoIO.RB_100b1:
message = "RB-100b1";
break;
case RoBoIO.RB_100b2:
message = "RB-100b2";
break;
case RoBoIO.RB_110:
message = "RB-110";
break;
case RoBoIO.RB_111:
message = "RB-111";
break;
default:
message = "No version";
break;
}
return(message);
}
// Method
//
public static bool Close()
{
if (_connected)
{
RoBoIO.i2c_Close();
_connected = false;
Log.WriteLineSucces("Closing: I2C lib");
return(true);
}
else
{
Log.WriteLineFail("Closing: I2C lib");
return(false);
}
}
//public static bool Init(UInt32 channels)
public static bool Init()
{
Log.Module = Module;
if (!_connected)
{
RoBoIO.roboio_SetRBVer(Server.MainIni.RoboardVersion);
Log.WriteLineSucces(string.Format("Set Roboard Version: {0}", Server.MainIni.RoboardVersionText));
}
if (!_connected)
{
for (int i = 0; i < StaticUtilities.numberOfServos; i++)
{
// The servo is KONDO_KRS788 and plugged on
RoBoIO.rcservo_SetServo(i, RoBoIO.RCSERVO_KONDO_KRS788);
// Set the servo state
RoBoIO.rcservo_SetServoParams1(i, 10000, 700, 2300);
// The servo has feedback functionality
RoBoIO.rcservo_SetServoType(i, RoBoIO.RCSERVO_SV_FEEDBACK, RoBoIO.RCSERVO_FB_SAFEMODE);
}
UInt32 channels = 0;
for (int j = 0; j < StaticUtilities.numberOfServos; j++)
{
if (MainIni.ChannelFunction[j] == 1)
{
channels += Convert.ToUInt32(Math.Pow(2, j));
}
}
Log.WriteLineMessage(string.Format("Channels : {0:X}", channels));
if (RoBoIO.rcservo_Initialize((UInt32)channels) == true)
{
Log.WriteLineSucces(string.Format("Opening: RCSERVO lib (for {0})", servo_name[servo_idx]));
RoBoIO.rcservo_EnterPlayMode();
RoBoIO.rcservo_EnableMPOS();
RoBoIO.rcservo_SetFPS(MainIni.FPS);
_connected = true;
return(true);
}
Log.WriteLineFail(string.Format("Opening: RCSERVO lib (for {0})", servo_name[servo_idx]));
_connected = false;
Log.WriteLineError(string.Format("RCSERVO lib fails to initialize ({0})", RoBoIO_DotNet.RoBoIO.roboio_GetErrMsg()));
return(false);
}
Log.WriteLineMessage(string.Format("Opening: RCSERVO lib (for {0})...already open", servo_name[servo_idx]));
return(true);
}
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);
}
// Method
//
public static bool Init()
{
Log.Module = Module;
if (!_connected)
{
if (RoBoIO.spi_Initialize(RoBoIO.SPICLK_21400KHZ))
{
_connected = true;
Log.WriteLineSucces("Opening: SPI lib");
return(true);
}
_connected = false;
Log.WriteLineFail("Opening: SPI lib");
Log.WriteLineError(string.Format("SPI lib fails to initialize ({0})", RoBoIO_DotNet.RoBoIO.roboio_GetErrMsg()));
return(false);
}
Log.WriteLineMessage("Opening: SPI lib...already open");
return(true);
}
// Method
//
public static bool Init()
{
Log.Module = Module;
if (!_connected)
{
if (RoBoIO.i2c_Initialize(RoBoIO.I2CIRQ_DISABLE))
{
_connected = true;
RoBoIO.i2c0_SetSpeed(RoBoIO.I2CMODE_FAST, i2c_clock);
Log.WriteLineSucces("Opening: I2C lib");
return(true);
}
_connected = false;
Log.WriteLineFail("Opening: I2C lib");
Log.WriteLineError(string.Format("I2C lib fails to initialize ({0})", RoBoIO_DotNet.RoBoIO.roboio_GetErrMsg()));
return(false);
}
Log.WriteLineMessage("Opening: I2C lib...already open");
return(true);
}
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);
}
// Method
//
public static void Close()
{
RoBoIO.spi_Close();
_connected = false;
Log.WriteLineSucces("Closing: SPI lib");
}
// Method
//
public static void Close()
{
RoBoIO.ad7918_CloseMCH();
_connected = false;
Log.WriteLineSucces("Closing AD7918 lib");
}
public static void Close()
{
RoBoIO.rcservo_Close();
_connected = false;
Log.WriteLineSucces("Closing RCSERVO lib");
}
