Пример #1
0
        /**
         * Setup all of the configuration parameters.
         */
        public int SetupConfig()
        {
            /* binary OR all the return values so we can make a quick decision if our init was successful */
            int status = 0;

            /* specify sensor characteristics */
            _talon.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
            _talon.SetSensorDirection(false); /* make sure positive motor output means sensor moves in position direction */
            // call ConfigEncoderCodesPerRev or ConfigPotentiometerTurns for Quadrature or Analog sensor types.

            /* brake or coast during neutral */
            status |= _talon.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);

            /* closed-loop and motion-magic parameters */
            status |= _talon.SetF(kSlotIdx, 0.1153f, kTimeoutMs); // 1300RPM (8874 native sensor units per 100ms) at full motor output (+1023)
            status |= _talon.SetP(kSlotIdx, 2.00f, kTimeoutMs);
            status |= _talon.SetI(kSlotIdx, 0f, kTimeoutMs);
            status |= _talon.SetD(kSlotIdx, 20f, kTimeoutMs);
            status |= _talon.SetIzone(kSlotIdx, 0, kTimeoutMs);
            status |= _talon.SelectProfileSlot(kSlotIdx); /* select this slot */
            status |= _talon.ConfigNominalOutputVoltage(0f, 0f, kTimeoutMs);
            status |= _talon.ConfigPeakOutputVoltage(+12f, -12f, kTimeoutMs);
            status |= _talon.SetMotionMagicCruiseVelocity(1000f, kTimeoutMs); // 1000 RPM
            status |= _talon.SetMotionMagicAcceleration(2000f, kTimeoutMs);   // 2000 RPM per sec, (0.5s to reach cruise velocity).

            /* Home the relative sensor,
             *  alternatively you can throttle until limit switch,
             *  use an absolute signal like CtreMagEncoder_Absolute or analog sensor.
             */
            status |= _talon.SetPosition(0, kTimeoutMs);

            return(status);
        }
Пример #2
0
        public void Run()
        {
            _talon.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);

            _talon.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
            _talon.SetSensorDirection(false);

            _talon.SetVoltageRampRate(0.0f);

            _talon.SetP(0, 0.80f);
            _talon.SetI(0, 0f);
            _talon.SetD(0, 0f);
            _talon.SetF(0, 0.09724488664269079041176191004297f);
            _talon.SelectProfileSlot(0);
            _talon.ConfigNominalOutputVoltage(0f, 0f);
            _talon.ConfigPeakOutputVoltage(+12.0f, -12.0f);
            _talon.ChangeMotionControlFramePeriod(5);

            /* loop forever */
            while (true)
            {
                _talon.GetMotionProfileStatus(out _motionProfileStatus);

                Drive();

                CTRE.Watchdog.Feed();

                Instrument();

                Thread.Sleep(5);
            }
        }
Пример #3
0
        uint [] _debLeftY = { 0, 0 }; // _debLeftY[0] is how many times leftY is zero, _debLeftY[1] is how many times leftY is not zeero.

        public void Run()
        {
            /* first choose the sensor */
            _talon.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
            _talon.SetSensorDirection(false);
            //_talon.ConfigEncoderCodesPerRev(XXX), // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder
            //_talon.ConfigPotentiometerTurns(XXX), // if using CTRE.TalonSrx.FeedbackDevice.AnalogEncoder or CTRE.TalonSrx.FeedbackDevice.AnalogPot

            /* set closed loop gains in slot0 */
            _talon.SetP(0, 0.2f); /* tweak this first, a little bit of overshoot is okay */
            _talon.SetI(0, 0f);
            _talon.SetD(0, 0f);
            _talon.SetF(0, 0f); /* For position servo kF is rarely used. Leave zero */

            /* use slot0 for closed-looping */
            _talon.SelectProfileSlot(0);

            /* set the peak and nominal outputs, 12V means full */
            _talon.ConfigNominalOutputVoltage(+0.0f, -0.0f);
            _talon.ConfigPeakOutputVoltage(+3.0f, -3.0f);

            /* how much error is allowed?  This defaults to 0. */
            _talon.SetAllowableClosedLoopErr(0, 0);

            /* zero the sensor and throttle */
            ZeroSensorAndThrottle();

            /* loop forever */
            while (true)
            {
                Loop10Ms();

                //if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR....
                if (_gamepad.GetButton(kEnableButton)) // check if bottom left shoulder buttom is held down.
                {
                    /* then enable motor outputs*/
                    CTRE.Watchdog.Feed();
                }

                /* print signals to Output window */
                Instrument();

                /* 10ms loop */
                Thread.Sleep(10);
            }
        }
Пример #4
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);
            }
        }
Пример #5
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();
            }
        }