Exemplo n.º 1
0
        //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);
        }