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