Ejemplo n.º 1
0
        public static void Main()
        {
            leftDrive  = new CTRE.TalonSrx(1);
            rightDrive = new CTRE.TalonSrx(2);
            leftDrive.SetInverted(true);

            leftDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
            rightDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
            leftDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            rightDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            leftDrive.ConfigEncoderCodesPerRev(560);
            rightDrive.ConfigEncoderCodesPerRev(560);
            leftDrive.SetSensorDirection(true);
            leftDrive.SetEncPosition(0);
            rightDrive.SetEncPosition(0);

            CTRE.Gamepad gamePad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
            /* loop forever */
            while (true)
            {
                if (gamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
                {
                    CheesyDrive(gamePad.GetAxis(1), gamePad.GetAxis(2));
                    CTRE.Watchdog.Feed();
                    Debug.Print("" + leftDrive.GetPosition() + ":" + rightDrive.GetPosition());
                }
                else
                {
                    Debug.Print("No Driver Pad");
                }
                /* wait a bit */
                System.Threading.Thread.Sleep(10);
            }
        }
Ejemplo n.º 2
0
        public override Int32 Begin()
        {
#if DEBUG
            Debug.Print(ToString() + " [BEGIN]");
#endif
            /* Setup Left and Right followers */
            RightSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
            RightSlave.Set(RIGHT_TALONSRX_ID);

            LeftSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
            LeftSlave.Set(LEFT_TALONSRX_ID);

            /* Configure the Left TalonSRX */
            Left.SetInverted(LEFT_INVT);
            Left.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
            Left.ConfigFwdLimitSwitchNormallyOpen(true);
            Left.ConfigRevLimitSwitchNormallyOpen(true);
            LeftSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
            LeftSlave.ConfigFwdLimitSwitchNormallyOpen(true);
            LeftSlave.ConfigRevLimitSwitchNormallyOpen(true);

            /* Configure the Right TalonSRX */
            Right.SetInverted(!LEFT_INVT);
            Right.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
            Right.ConfigFwdLimitSwitchNormallyOpen(true);
            Right.ConfigRevLimitSwitchNormallyOpen(true);
            RightSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
            RightSlave.ConfigFwdLimitSwitchNormallyOpen(true);
            RightSlave.ConfigRevLimitSwitchNormallyOpen(true);

            if (USE_SPEED_MODE)
            {
                /* Setup Left and Right leaders with PID */
                Right.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
                Right.SetSensorDirection(false);
                Right.ConfigEncoderCodesPerRev(RIGHT_EncTPR);

                Right.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
                Right.SetPID(0, RIGHT_P, RIGHT_I, RIGHT_D);
                Right.SetF(0, RIGHT_F);

                Left.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
                Left.SetSensorDirection(false);
                Left.ConfigEncoderCodesPerRev(LEFT_EncTPR);

                Left.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
                Left.SetPID(0, LEFT_P, LEFT_I, LEFT_D);
                Left.SetF(0, LEFT_F);
            }

            /* Enable the TalonSRXs */
            Right.Enable();
            RightSlave.Enable();
            Left.Enable();
            LeftSlave.Enable();
            return(0);
        }
Ejemplo n.º 3
0
        public static void Main()
        {
            CTRE.TalonSrx leftMotor  = new CTRE.TalonSrx(1);
            CTRE.TalonSrx rightMotor = new CTRE.TalonSrx(2);
            CTRE.TalonSrx scoopMotor = new CTRE.TalonSrx(3);
            CTRE.TalonSrx depthMotor = new CTRE.TalonSrx(4);
            CTRE.TalonSrx winchMotor = new CTRE.TalonSrx(5);

            leftMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            rightMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            scoopMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            depthMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
            winchMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);

            leftMotor.SetSensorDirection(false);
            rightMotor.SetSensorDirection(true);
            scoopMotor.SetSensorDirection(false);
            depthMotor.SetSensorDirection(false);
            winchMotor.SetSensorDirection(false);

            leftMotor.ConfigEncoderCodesPerRev(80);
            rightMotor.ConfigEncoderCodesPerRev(80);
            scoopMotor.ConfigEncoderCodesPerRev(80);
            depthMotor.ConfigEncoderCodesPerRev(80);
            winchMotor.ConfigEncoderCodesPerRev(80);

            leftMotor.SetP(0, 0.35F);
            leftMotor.SetI(0, 0.0F);
            leftMotor.SetD(0, 0.0F);
            leftMotor.SetF(0, 0.0F);
            leftMotor.SelectProfileSlot(0);

            rightMotor.SetP(0, 0.35F);
            rightMotor.SetI(0, 0.0F);
            rightMotor.SetD(0, 0.0F);
            rightMotor.SetF(0, 0.0F);
            rightMotor.SelectProfileSlot(0);

            scoopMotor.SetP(0, 0.6F);
            scoopMotor.SetI(0, 0.0F);
            scoopMotor.SetD(0, 0.0F);
            scoopMotor.SetF(0, 0.0F);
            scoopMotor.SelectProfileSlot(0);

            depthMotor.SetP(0, 0.6F);
            depthMotor.SetI(0, 0.0F);
            depthMotor.SetD(0, 0.0F);
            depthMotor.SetF(0, 0.0F);
            depthMotor.SelectProfileSlot(0);

            winchMotor.SetP(0, 0.6F);
            winchMotor.SetI(0, 0.0F);
            winchMotor.SetD(0, 0.0F);
            winchMotor.SetF(0, 0.0F);
            winchMotor.SelectProfileSlot(0);

            leftMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
            rightMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
            scoopMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
            depthMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
            winchMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);

            leftMotor.SetAllowableClosedLoopErr(0, 0);
            rightMotor.SetAllowableClosedLoopErr(0, 0);
            scoopMotor.SetAllowableClosedLoopErr(0, 0);
            depthMotor.SetAllowableClosedLoopErr(0, 0);
            winchMotor.SetAllowableClosedLoopErr(0, 0);

            leftMotor.SetPosition(0);
            rightMotor.SetPosition(0);
            scoopMotor.SetPosition(0);
            depthMotor.SetPosition(0);
            winchMotor.SetPosition(0);

            leftMotor.SetVoltageRampRate(0);
            rightMotor.SetVoltageRampRate(0);
            scoopMotor.SetVoltageRampRate(0);
            depthMotor.SetVoltageRampRate(0);
            winchMotor.SetVoltageRampRate(0);

            ArrayList motorSetpointData = new ArrayList();
            ArrayList motorStatusData   = new ArrayList();
            ArrayList talons            = new ArrayList();

            talons.Add(leftMotor);
            talons.Add(rightMotor);
            talons.Add(scoopMotor);
            talons.Add(depthMotor);
            talons.Add(winchMotor);

            String inboundMessageStr  = "";
            String outboundMessageStr = "";

            SetpointData leftMotorSetpointData  = new SetpointData(1, 0, 0.0F);
            SetpointData rightMotorSetpointData = new SetpointData(2, 0, 0.0F);
            SetpointData scoopMotorSetpointData = new SetpointData(3, 0, 0.0F);
            SetpointData depthMotorSetpointData = new SetpointData(4, 0, 0.0F);
            SetpointData winchMotorSetpointData = new SetpointData(5, 0, 0.0F);

            StatusData leftMotorStatusData  = new StatusData(1, leftMotor);
            StatusData rightMotorStatusData = new StatusData(2, rightMotor);
            StatusData scoopMotorStatusData = new StatusData(3, scoopMotor);
            StatusData depthMotorStatusData = new StatusData(4, depthMotor);
            StatusData winchMotorStatusData = new StatusData(5, winchMotor);

            motorSetpointData.Add(leftMotorSetpointData);
            motorSetpointData.Add(rightMotorSetpointData);
            motorSetpointData.Add(scoopMotorSetpointData);
            motorSetpointData.Add(depthMotorSetpointData);
            motorSetpointData.Add(winchMotorSetpointData);

            motorStatusData.Add(leftMotorStatusData);
            motorStatusData.Add(rightMotorStatusData);
            motorStatusData.Add(scoopMotorStatusData);
            motorStatusData.Add(depthMotorStatusData);
            motorStatusData.Add(winchMotorStatusData);

            CTRE.Watchdog.Feed();

            uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
            uart.Open();


            while (true)
            {
                //read whatever is available from the UART into the inboundMessageStr
                motorSetpointData = readUART(ref inboundMessageStr);
                CTRE.Watchdog.Feed();
                //if any of the talon positions need to be reset, this will reset them
                resetEncoderPositions(talons);
                CTRE.Watchdog.Feed();
                //attempt to process whatever was contained in the most recent message
                processInboundData(motorSetpointData, talons);
                CTRE.Watchdog.Feed();
                //get a bunch of data from the motors in their current states
                updateMotorStatusData(motorStatusData);
                CTRE.Watchdog.Feed();
                //package that motor data into a formatted message
                outboundMessageStr = makeOutboundMessage(motorStatusData);
                CTRE.Watchdog.Feed();
                //send that message back to the main CPU
                writeUART(outboundMessageStr);

                //Debug.Print("set=" + winchMotor.GetSetpoint().ToString() + " mod=" + winchMotor.GetControlMode().ToString() +
                //            "pos=" + winchMotor.GetPosition().ToString() + " vel=" + winchMotor.GetSpeed().ToString() +
                //            "err=" + winchMotor.GetClosedLoopError().ToString() + "vlt=" + winchMotor.GetOutputVoltage());
                //Debug.Print(DateTime.Now.ToString());
                CTRE.Watchdog.Feed();
                //keep the loop timing consistent //TODO: evaluate if this is necessary
                //System.Threading.Thread.Sleep(10);
            }
        }
Ejemplo n.º 4
0
        public static void Main()
        {
            //Set up the motors
            CTRE.TalonSrx motor = new CTRE.TalonSrx(1);

/*          CTRE.TalonSrx rightMotor = new CTRE.TalonSrx(2);
 *          CTRE.TalonSrx scoopMotor = new CTRE.TalonSrx(3);
 *          CTRE.TalonSrx depthMotor = new CTRE.TalonSrx(4);
 *          CTRE.TalonSrx winchMotor = new CTRE.TalonSrx(5);
 */
            //Set encoders for each motor
            motor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);

/*
 *          rightMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
 *          scoopMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
 *          depthMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
 *          winchMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
 */
            //Set direction of the encoders
            motor.SetSensorDirection(false);

/*          rightMotor.SetSensorDirection(true);
 *          scoopMotor.SetSensorDirection(false);
 *          depthMotor.SetSensorDirection(false);
 *          winchMotor.SetSensorDirection(false);
 */
            //Set Ticks per Rev of the encoders
            motor.ConfigEncoderCodesPerRev(80);

/*          rightMotor.ConfigEncoderCodesPerRev(80);
 *          scoopMotor.ConfigEncoderCodesPerRev(80);
 *          depthMotor.ConfigEncoderCodesPerRev(80);
 *          winchMotor.ConfigEncoderCodesPerRev(80);
 */
            //Sets PIDF values for each motor//
            motor.SetP(0, 0.35F);
            motor.SetI(0, 0.0F);
            motor.SetD(0, 0.0F);
            motor.SetF(0, 0.0F);
            motor.SelectProfileSlot(0);

/*            rightMotor.SetP(0, 0.35F);
 *          rightMotor.SetI(0, 0.0F);
 *          rightMotor.SetD(0, 0.0F);
 *          rightMotor.SetF(0, 0.0F);
 *          rightMotor.SelectProfileSlot(0);
 *
 *          scoopMotor.SetP(0, 0.6F);
 *          scoopMotor.SetI(0, 0.0F);
 *          scoopMotor.SetD(0, 0.0F);
 *          scoopMotor.SetF(0, 0.0F);
 *          scoopMotor.SelectProfileSlot(0);
 *
 *          depthMotor.SetP(0, 0.6F);
 *          depthMotor.SetI(0, 0.0F);
 *          depthMotor.SetD(0, 0.0F);
 *          depthMotor.SetF(0, 0.0F);
 *          depthMotor.SelectProfileSlot(0);
 *
 *          winchMotor.SetP(0, 0.6F);
 *          winchMotor.SetI(0, 0.0F);
 *          winchMotor.SetD(0, 0.0F);
 *          winchMotor.SetF(0, 0.0F);
 *          winchMotor.SelectProfileSlot(0);
 *          //////////////////////////////////
 */

            //Sets Nominal Output Voltage for each motor
            motor.ConfigNominalOutputVoltage(+0.0F, -0.0F);

            /*rightMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
             * scoopMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
             * depthMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
             * winchMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F);
             */
            // Set allowed error for closed loop feedback
            motor.SetAllowableClosedLoopErr(0, 0);

            /*rightMotor.SetAllowableClosedLoopErr(0, 0);
             * scoopMotor.SetAllowableClosedLoopErr(0, 0);
             * depthMotor.SetAllowableClosedLoopErr(0, 0);
             * winchMotor.SetAllowableClosedLoopErr(0, 0);
             */
            //Set Initial position of the motors
            motor.SetPosition(0);

            /* rightMotor.SetPosition(0);
             * scoopMotor.SetPosition(0);
             * depthMotor.SetPosition(0);
             * winchMotor.SetPosition(0);
             */
            //Sets Voltage Ramp rate of each motor
            motor.SetVoltageRampRate(0);

            /*rightMotor.SetVoltageRampRate(0);
             * scoopMotor.SetVoltageRampRate(0);
             * depthMotor.SetVoltageRampRate(0);
             * winchMotor.SetVoltageRampRate(0);
             */


            ArrayList motorSetpointData = new ArrayList();
            ArrayList motorStatusData   = new ArrayList();
            ArrayList talons            = new ArrayList();

            //Add talons to each motor
            talons.Add(motor);

            /*talons.Add(rightMotor);
             * talons.Add(scoopMotor);
             * talons.Add(depthMotor);
             * talons.Add(winchMotor);
             */
            //Initializes inbound and outbound message strings to empty
            String inboundMessageStr  = "";
            String outboundMessageStr = "";

            //Initializes and adds the SetpointData and the StatusData for each motor (ID, mode, setpoint//
            SetpointData motor = new SetpointData(1, 0, 0.0F);

            /*SetpointData rightMotorSetpointData = new SetpointData(2, 0, 0.0F);
             * SetpointData scoopMotorSetpointData = new SetpointData(3, 0, 0.0F);
             * SetpointData depthMotorSetpointData = new SetpointData(4, 0, 0.0F);
             * SetpointData winchMotorSetpointData = new SetpointData(5, 0, 0.0F);
             */
            StatusData motor = new StatusData(1, motor);

            /*StatusData rightMotorStatusData = new StatusData(2, rightMotor);
             * StatusData scoopMotorStatusData = new StatusData(3, scoopMotor);
             * StatusData depthMotorStatusData = new StatusData(4, depthMotor);
             * StatusData winchMotorStatusData = new StatusData(5, winchMotor);
             */
            motorSetpointData.Add(motor);

            /*motorSetpointData.Add(rightMotorSetpointData);
             * motorSetpointData.Add(scoopMotorSetpointData);
             * motorSetpointData.Add(depthMotorSetpointData);
             * motorSetpointData.Add(winchMotorSetpointData);
             */
            motorStatusData.Add(motor);

            /*motorStatusData.Add(rightMotorStatusData);
            *  motorStatusData.Add(scoopMotorStatusData);
            *  motorStatusData.Add(depthMotorStatusData);
            *  motorStatusData.Add(winchMotorStatusData);*/
            //////////////////////////////////////////

            CTRE.Watchdog.Feed();

            //Open up UART communication to the linux box
            uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
            uart.Open();

            //The loop
            while (true)
            {
                //read whatever is available from the UART into the inboundMessageStr
                motorSetpointData = readUART(ref inboundMessageStr);
                CTRE.Watchdog.Feed();

                //if any of the talon positions need to be reset, this will reset them
                resetEncoderPositions(talons);
                CTRE.Watchdog.Feed();

                //attempt to process whatever was contained in the most recent message
                processInboundData(motorSetpointData, talons);
                CTRE.Watchdog.Feed();

                //get a bunch of data from the motors in their current states
                updateMotorStatusData(motorStatusData);
                CTRE.Watchdog.Feed();

                //package that motor data into a formatted message
                outboundMessageStr = makeOutboundMessage(motorStatusData);
                CTRE.Watchdog.Feed();

                //send that message back to the main CPU
                writeUART(outboundMessageStr);
                CTRE.Watchdog.Feed();
            }
        }