コード例 #1
0
        static void Drive()
        {
            if (null == _gamepad)
            {
                _gamepad = new GameController(UsbHostDevice.GetInstance());
            }

            float x     = _gamepad.GetAxis(0);
            float y     = -1 * _gamepad.GetAxis(1);
            float twist = _gamepad.GetAxis(2);

            Deadband(ref x);
            Deadband(ref y);
            Deadband(ref twist);

            float leftThrot  = y + twist;
            float rightThrot = y - twist;

            left.Set(ControlMode.PercentOutput, leftThrot);
            //leftSlave.Set(ControlMode.PercentOutput, leftThrot);
            right.Set(ControlMode.PercentOutput, -rightThrot);
            //rightSlave.Set(ControlMode.PercentOutput, -rightThrot);

            stringBuilder.Append("\t");
            stringBuilder.Append(x);
            stringBuilder.Append("\t");
            stringBuilder.Append(y);
            stringBuilder.Append("\t");
            stringBuilder.Append(twist);
        }
コード例 #2
0
        public static void Main()
        {
            //Gamepad for input
            GameController _gamepad = new GameController(UsbHostDevice.GetInstance());

            //Create the DriverModule object by giving it the port you plugged it in to.
            DriverModule driver = new DriverModule(CTRE.HERO.IO.Port5);

            //these just act as shorter names for the driveLow and pullUp states
            bool driveLow = DriverModule.OutputState.driveLow;
            bool pullUp   = DriverModule.OutputState.pullUp;

            while (true)
            {
                //When the 'X' button is pressed, enable outputs
                if (_gamepad.GetButton(1) == true)
                {
                    driver.Set(1, driveLow);
                    driver.Set(2, driveLow);
                }
                else
                {
                    driver.Set(1, pullUp);
                    driver.Set(2, pullUp);
                }

                System.Threading.Thread.Sleep(10);
            }
        }
コード例 #3
0
ファイル: Program.cs プロジェクト: CoryNessCTR/HERO-Examples
        public static void Main()
        {
            TalonSrx       test  = new TalonSrx(0);
            GameController stick = new GameController(UsbHostDevice.GetInstance(0), 0);

            /* loop forever */
            while (true)
            {
                if (stick.GetConnectionStatus() == UsbDeviceConnection.Connected)
                {
                    CTRE.Watchdog.Feed();
                }

                //This call is redundant but you can un-comment to guarantee limit switches will work or change the mode.
                //test.ConfigLimitMode(TalonSrx.LimitMode.kLimitMode_SwitchInputsOnly);

                Debug.Print("Rev: " + test.IsRevLimitSwitchClosed() + "  | Fwd: " + test.IsFwdLimitSwitchClosed());
                Debug.Print("" + test.GetFaults());

                if (stick.GetButton(1))
                {
                    test.ClearStickyFaults();
                }


                test.Set(stick.GetAxis(1));

                /* wait a bit */
                System.Threading.Thread.Sleep(10);
            }
        }
コード例 #4
0
 public void Acquire()
 {
     if (this.controller == null)
     {
         this.controller  = new GameController(UsbHostDevice.GetInstance());
         this.IsConnected = this.controller != null;
     }
 }
コード例 #5
0
ファイル: Program.cs プロジェクト: FRC2539/KryptonHERO
        public static Button(uint id, dummy execute, dummy end)   // to assign to button, cannot take or return values.
        {
            buttonID = id;
            toDo     = execute;
            onStop   = end;

            if (controller == null)
            {
                controller = new GameController(UsbHostDevice.GetInstance());
            }                                                                                        // Create controller.
        }
コード例 #6
0
ファイル: Robot.cs プロジェクト: FRC-Team2655/DemoBot
        private void robotInit()
        {
            js0 = new GameController(UsbHostDevice.GetInstance());
            leftSlave.Follow(leftMaster);
            rightSlave.Follow(rightMaster);
            shooterWheelSlave.Follow(shooterWheelMaster);

            rightMaster.SetInverted(true);
            rightSlave.SetInverted(true);
            bottomIntake.SetInverted(true);
        }
コード例 #7
0
        private void robotInit()
        {
            js0 = new GameController(UsbHostDevice.GetInstance());
            leftSlave1.Follow(leftMaster);
            leftSlave2.Follow(leftMaster);
            rightSlave1.Follow(rightMaster);
            rightSlave2.Follow(rightMaster);

            intake2.Follow(intake1);

            rightMaster.SetInverted(true);
            rightSlave1.SetInverted(true);
            rightSlave2.SetInverted(true);
        }
コード例 #8
0
        public static void Drive(GameController GAMEPAD, StringBuilder stringBuilder)
        {
            /*Talon and Encoder Constants*/
            right.SetNeutralMode(NeutralMode.Brake);
            //rightSlave.SetNeutralMode(NeutralMode.Brake);
            left.SetNeutralMode(NeutralMode.Brake);
            //leftSlave.SetNeutralMode(NeutralMode.Brake);

            /*Right side of drivetrain needs to be inverted*/
            right.SetInverted(true);
            //rightSlave.SetInverted(true);



            //left.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder,Helpers.PID,Helpers.timeoutMs);
            //leftSlave.ConfigRemoteFeedbackFilter(left.GetDeviceID(), RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, Helpers.remotePID, Helpers.timeoutMs);
            //right.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, Helpers.PID, Helpers.timeoutMs);
            //rightSlave.ConfigRemoteFeedbackFilter(left.GetDeviceID(), RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, Helpers.remotePID, Helpers.timeoutMs);

            /*End Constants*/

            if (null == GAMEPAD)
            {
                GAMEPAD = new GameController(UsbHostDevice.GetInstance(0));
            }

            double x = GAMEPAD.GetAxis(1);
            double y = GAMEPAD.GetAxis(3);

            //Helpers.Deadband(ref x);
            //Helpers.Deadband(ref y);

            //Pow(x,2) gives finer controls over the drivebase
            //.5 for total half-speed reduction
            //sign(x) returns the sign, which is useful since the pow removes the negative sign.
            double leftThrot  = (System.Math.Pow(x, 2)) * .5 * System.Math.Sign(x);
            double rightThrot = (System.Math.Pow(y, 2)) * .5 * System.Math.Sign(y);

            //TODO
            //Uncomment when ready to test on a robot
            left.Set(ControlMode.PercentOutput, leftThrot);
            //leftSlave.Set(ControlMode.PercentOutput, leftThrot);
            right.Set(ControlMode.PercentOutput, -rightThrot);
            //rightSlave.Set(ControlMode.PercentOutput, -rightThrot);

            stringBuilder.Append("\t");
            stringBuilder.Append(leftThrot);
            stringBuilder.Append("\t");
            stringBuilder.Append(rightThrot);
        }
コード例 #9
0
        public TeleopControl()
        {
            m_gamepad = new GameController(UsbHostDevice.GetInstance()); // create gamepad

            m_chassis = Chassis.GetInstance();                           //Creates chassis

            m_deliver = Deliver.GetInstance();                           // Create deliver

            m_transfer = Transfer.GetInstance();                         //Creates "Transfer"

            m_intake = Intake.GetInstance();                             // Creates Intake

            m_elevator = FlagElevator.GetInstance();                     //Creates Flag Elevator

            m_grabber   = FlagGrabber.GetInstance();                     //Creates Flag Grabber
            prevDeliver = false;
        }
コード例 #10
0
ファイル: Program.cs プロジェクト: thatnerdjack/botshot-c
        static void Operate()
        {
            if (_gamepad == null)
            {
                _gamepad = new CTRE.Phoenix.Controller.GameController(UsbHostDevice.GetInstance(0));
            }
            //Drive Function
            stringBuilder.Append("--DRIVEBASE CONTROLS--");
            DriveSub.Drive(_gamepad, stringBuilder);

            //Shooting Function
            stringBuilder.Append("\n--SHOOTER CONTROLS--");
            ShootSub.Shoot(_gamepad, stringBuilder);

            //Tilt Function
            stringBuilder.Append("\n--TILT CONTROLS--");
            ShootSub.Tilt(_gamepad, stringBuilder);
        }
コード例 #11
0
        static Hardware()
        {
            _leftDrive  = new VictorSPX(1);
            _rightDrive = new VictorSPX(2);
            _lid        = new TalonSRX(9);

            _leftDrive.ConfigFactoryDefault();
            _rightDrive.ConfigFactoryDefault();
            _lid.ConfigFactoryDefault();

            _lid.ConfigForwardLimitSwitchSource(CTRE.Phoenix.MotorControl.LimitSwitchSource.FeedbackConnector, CTRE.Phoenix.MotorControl.LimitSwitchNormal.NormallyOpen);
            _lid.ConfigReverseLimitSwitchSource(CTRE.Phoenix.MotorControl.LimitSwitchSource.FeedbackConnector, CTRE.Phoenix.MotorControl.LimitSwitchNormal.NormallyOpen);
            _lid.ConfigContinuousCurrentLimit(2);

            // on port 3: Hardware Pin 1 is CTRE.HERO.IO.Port3.Pin3.
            //            Hardware Pin 6 is CTRE.HERO.IO.Port3.Pin8.
            _testOutputPort = new OutputPort(CTRE.HERO.IO.Port5.Pin8, false);

            b0_supply = new OutputPort(CTRE.HERO.IO.Port3.Pin3, false);
            b0_tank   = new OutputPort(CTRE.HERO.IO.Port3.Pin4, false);
            b0_shot   = new OutputPort(CTRE.HERO.IO.Port3.Pin5, false);
            b1_supply = new OutputPort(CTRE.HERO.IO.Port3.Pin6, false);
            b1_tank   = new OutputPort(CTRE.HERO.IO.Port3.Pin7, false);
            b1_shot   = new OutputPort(CTRE.HERO.IO.Port3.Pin8, false);

            voltage = new AnalogInput(CTRE.HERO.IO.Port8.Analog_Pin5);

            lid_limit_switch     = new InputPort(CTRE.HERO.IO.Port8.Pin6, false, Port.ResistorMode.PullUp);
            lid_limit_switch_led = new OutputPort(CTRE.HERO.IO.Port5.Pin3, false);

            UsbHostDevice usb = CTRE.Phoenix.UsbHostDevice.GetInstance();

            _gamepad = new GameController(usb);
            usb.SetSelectableXInputFilter(UsbHostDevice.SelectableXInputFilter.XInputDevices);

            _display = new Display();
        }
コード例 #12
0
        static void Drive()
        {
            if (null == _gamepad)
            {
                _gamepad = new GameController(UsbHostDevice.GetInstance());
            }

            float x    = _gamepad.GetAxis(0);      // Positive is strafe-right, negative is strafe-left
            float y    = -1 * _gamepad.GetAxis(1); // Positive is forward, negative is reverse
            float turn = _gamepad.GetAxis(2);      // Positive is turn-right, negative is turn-left

            Deadband(ref x);
            Deadband(ref y);
            Deadband(ref turn);

            float leftFrnt_throt = y + x + turn; // left front moves positive for forward, strafe-right, turn-right
            float leftRear_throt = y - x + turn; // left rear moves positive for forward, strafe-left, turn-right
            float rghtFrnt_throt = y - x - turn; // right front moves positive for forward, strafe-left, turn-left
            float rghtRear_throt = y + x - turn; // right rear moves positive for forward, strafe-right, turn-left

            /* normalize here, there a many way to accomplish this, this is a simple solution */
            Normalize(ref leftFrnt_throt);
            Normalize(ref leftRear_throt);
            Normalize(ref rghtFrnt_throt);
            Normalize(ref rghtRear_throt);

            /* everything up until this point assumes positive spins motor so that robot moves forward.
             *  But typically one side of the robot has to drive negative (red LED) to move robor forward.
             *  Assuming the left-side has to be negative to move robot forward, flip the left side */
            leftFrnt_throt *= -1;
            leftRear_throt *= -1;

            leftFrnt.Set(ControlMode.PercentOutput, leftFrnt_throt);
            leftRear.Set(ControlMode.PercentOutput, leftRear_throt);
            rghtFrnt.Set(ControlMode.PercentOutput, rghtFrnt_throt);
            rghtRear.Set(ControlMode.PercentOutput, rghtRear_throt);
        }
コード例 #13
0
ファイル: Robot.cs プロジェクト: umrest/HERO-Code-2018
 public Robot()
 {
     GameController _gamepad = new GameController(UsbHostDevice.GetInstance());
 }
コード例 #14
0
        public static void Main()
        {
            var casterDrive = new CasterDrive();

            // uncomment this if you need to zero the swerves

            /*while (true)
             * {
             *  Debug.Print("Angle: " + casterDrive.casters[0].Angle + "\t" + casterDrive.casters[1].Angle + "\t" + casterDrive.casters[2].Angle);
             * }*/

            var cannon = new Cannon();
            // TODO: Move controller stuff to operator input class
            var controller = new GameController(UsbHostDevice.GetInstance());

            Debug.Print("Program started");

            var controllerValues = new GameControllerValues();

            var stopwatch        = new Stopwatch();
            var watchdogListener = new Listener(false);
            var shootModeToggler = new Toggler(true);
            var shootListener    = new Listener(true);
            var zeroListener     = new Listener(false);

            while (true)
            {
                double dt = stopwatch.Duration;
                stopwatch.Start();
                controller.GetAllValues(ref controllerValues);

                bool isEnabled = controller.GetButton(5);
                if (isEnabled && controller.GetConnectionStatus() == UsbDeviceConnection.Connected)
                {
                    Watchdog.Feed();
                }

                double turn = -controller.GetAxis(2);
                casterDrive.Drive(new Vector2(-controller.GetAxis(0), controller.GetAxis(1)), turn);

                bool shootMode = shootModeToggler.Get(isEnabled && controller.GetButton(1));

                if (zeroListener.Get(controller.GetButton(3)))
                {
                    Debug.Print("Zeroing Gyro");
                    casterDrive.ZeroGyro();
                }

                bool firing = isEnabled && shootListener.Get(controller.GetButton(6));

                if (watchdogListener.Get(Watchdog.IsEnabled()))
                {
                    cannon.Setpoint = cannon.Angle;
                }

                double adjust;
                if (controllerValues.pov == 0)
                {
                    adjust = 30.0 * dt;
                }
                else if (controllerValues.pov == 4)
                {
                    adjust = -30.0 * dt;
                }
                else
                {
                    adjust = 0.0;
                }

                cannon.Update(shootMode, firing, adjust);

                //Debug.Print(controllerValues.pov +"");
                //Debug.Print(dt +"");
                //cannon.DebugPID();

                /*
                 * leftMax = Math.Max(leftMax, casterDrive.casters[0].TurnCurrent);
                 * rightMax = Math.Max(rightMax, casterDrive.casters[1].TurnCurrent);
                 * backMax = Math.Max(backMax, casterDrive.casters[2].TurnCurrent);
                 * Debug.Print("Left: " + leftMax + "\tRight: " + rightMax + "\tBack: " + backMax);
                 */
            }
        }
コード例 #15
0
        public void RunForever()
        {
            /* enable XInput, if gamepad is in DInput it will disable robot.  This way you can
             * use X mode for drive, and D mode for disable (instead of vice versa as the
             * stock HERO implementation traditionally does). */
            UsbHostDevice.GetInstance(0).SetSelectableXInputFilter(UsbHostDevice.SelectableXInputFilter.XInputDevices);

            while (true)
            {
                if (_gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
                {
                    Watchdog.Feed();
                }

                /* get buttons */
                bool[] btns = new bool[_buttons.Length];
                for (uint i = 1; i < 20; ++i)
                {
                    btns[i] = _gamepad.GetButton(i);
                }

                /* get sticks */
                for (uint i = 0; i < _sticks.Length; ++i)
                {
                    _sticks[i] = _gamepad.GetAxis(i);
                }

                /* yield for a bit, and track timeouts */
                System.Threading.Thread.Sleep(10);
                if (_rumblingTimeMs < 5000)
                {
                    _rumblingTimeMs += 10;
                }

                /* update the Talon using the shoulder analog triggers */
                _tal.Set(ControlMode.PercentOutput, (_sticks[5] - _sticks[4]) * 0.60f);

                /* fire some solenoids based on buttons */
                _driver.Set(0, _buttons[1]);
                _driver.Set(1, _buttons[2]);
                _driver.Set(2, _buttons[3]);
                _driver.Set(3, _buttons[4]);

                /* rumble state machine */
                switch (_rumblinSt)
                {
                /* rumbling is disabled, require some off time to save battery */
                case 0:
                    _gamepad.SetLeftRumble(0);
                    _gamepad.SetRightRumble(0);

                    if (_rumblingTimeMs < 100)
                    {
                        /* waiting for off-time */
                    }
                    else if ((btns[1] && !_buttons[1]))     /* button off => on */
                    {
                        /* off time long enough, user pressed btn */
                        _rumblingTimeMs = 0;
                        _rumblinSt      = 1;
                        _gamepad.SetLeftRumble(0xFF);
                    }
                    else if ((btns[2] && !_buttons[2]))     /* button off => on */
                    {
                        /* off time long enough, user pressed btn */
                        _rumblingTimeMs = 0;
                        _rumblinSt      = 1;
                        _gamepad.SetRightRumble(0xFF);
                    }
                    break;

                /* already vibrating, track the time */
                case 1:
                    if (_rumblingTimeMs > 500)
                    {
                        /* vibrating too long, turn off now */
                        _rumblingTimeMs = 0;
                        _rumblinSt      = 0;
                        _gamepad.SetLeftRumble(0);
                        _gamepad.SetRightRumble(0);
                    }
                    else if ((btns[3] && !_buttons[3]))      /* button off => on */
                    {
                        /* immedietely turn off */
                        _rumblingTimeMs = 0;
                        _rumblinSt      = 0;
                        _gamepad.SetLeftRumble(0);
                        _gamepad.SetRightRumble(0);
                    }
                    else if ((btns[1] && !_buttons[1]))    /* button off => on */
                    {
                        _gamepad.SetLeftRumble(0xFF);
                    }
                    else if ((btns[2] && !_buttons[2]))     /* button off => on */
                    {
                        _gamepad.SetRightRumble(0xFF);
                    }
                    break;
                }

                /* this will likley be replaced with a strongly typed interface,
                 * control the LEDs on the center XBOX emblem. */
                if (btns[5] && !_buttons[5])
                {
                    _gamepad.SetLEDCode(6);
                }
                if (btns[6] && !_buttons[6])
                {
                    _gamepad.SetLEDCode(7);
                }
                if (btns[7] && !_buttons[7])
                {
                    _gamepad.SetLEDCode(8);
                }
                if (btns[8] && !_buttons[8])
                {
                    _gamepad.SetLEDCode(9);
                }

                /* build line to print */
                StringBuilder sb = new StringBuilder();
                foreach (float stick in _sticks)
                {
                    sb.Append(Format(stick));
                    sb.Append(",");
                }

                sb.Append("-");
                for (uint i = 1; i < _buttons.Length; ++i)
                {
                    if (_buttons[i])
                    {
                        sb.Append("b" + i + ",");
                    }
                }

                /* print useful info */
                sb.AppendLine();
                Debug.Print(sb.ToString());

                /* save button states for button-change states */
                _buttons = btns;
            }
        }
コード例 #16
0
        public static void Main()
        {
            // Game Controller
            GameController gamepad = new GameController(UsbHostDevice.GetInstance());

            // NinaB Font
            Font ninaB = Properties.Resources.GetFont(Properties.Resources.FontResources.NinaB);

            // Initializing a display module: DisplayModule(port, orientation)
            DisplayModule displayModule = new DisplayModule(CTRE.HERO.IO.Port8, DisplayModule.OrientationType.Landscape);

            while (true)
            {
                // Connect the game controller first so that the sprites show up
                if (gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
                {
                    // Erases everything on the display
                    displayModule.Clear();

                    // Adding labels: [Display Module Name].AddLabelSprite(font, colour, x_pos, y_pos, width, height)
                    DisplayModule.LabelSprite title   = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 27, 17, 120, 15);
                    DisplayModule.LabelSprite x_label = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 65, 80, 15);
                    DisplayModule.LabelSprite y_label = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 80, 85, 80, 15);

                    // Adding rectangles: [Display Module Name].AddRectSprite(colour, x_pos, y_pos, width, height)
                    DisplayModule.RectSprite x_rect = displayModule.AddRectSprite(DisplayModule.Color.White, 20, 55, 18, 55);
                    DisplayModule.RectSprite y_rect = displayModule.AddRectSprite(DisplayModule.Color.White, 47, 55, 18, 55);

                    // Everything gets cleared when the game controller is unplugged
                    while (gamepad.GetConnectionStatus() == UsbDeviceConnection.Connected)
                    {
                        // Declares and resets the joystick
                        double x_value = gamepad.GetAxis(0);
                        double y_value = -gamepad.GetAxis(1);
                        if (x_value < 0.05 && x_value > -0.05)
                        {
                            x_value = 0;
                        }
                        if (y_value < 0.05 && y_value > -0.05)
                        {
                            y_value = 0;
                        }

                        // Changes the color of the rectangle (x-value of the left joystick): [Rectangle Name].SetColor(colour)
                        if (x_value > 0.05)
                        {
                            x_rect.SetColor(DisplayModule.Color.Green);
                        }
                        else if (x_value < -0.05)
                        {
                            x_rect.SetColor(DisplayModule.Color.Red);
                        }
                        else
                        {
                            x_rect.SetColor(DisplayModule.Color.White);
                        }

                        // Changes the color of the rectangle (y-value of the left joystick): [Rectangle Name].SetColor(colour)
                        if (y_value > 0.05)
                        {
                            y_rect.SetColor(DisplayModule.Color.Green);
                        }
                        else if (y_value < -0.05)
                        {
                            y_rect.SetColor(DisplayModule.Color.Red);
                        }
                        else
                        {
                            y_rect.SetColor(DisplayModule.Color.White);
                        }

                        // Sets the text that the label displays: [Label Name].SetText(text: string)
                        title.SetText("Joystick Control");
                        x_label.SetText("X: " + x_value.ToString());
                        y_label.SetText("Y: " + y_value.ToString());
                    }
                }
                else
                {
                    // Erases everything on the display
                    displayModule.Clear();

                    // Adding images: [Display Module Name].AddResourceImageSprite(resource_manager, img_ID, img_type, x_pos, y_pos)
                    DisplayModule.ResourceImageSprite image = displayModule.AddResourceImageSprite(Properties.Resources.ResourceManager, Properties.Resources.BinaryResources.img, Bitmap.BitmapImageType.Jpeg, 44, 16);

                    // Adding labels: [Display Module Name].AddLabelSprite(font, colour, x_pos, y_pos, width, height)
                    DisplayModule.LabelSprite text = displayModule.AddLabelSprite(ninaB, DisplayModule.Color.White, 36, 99, 100, 30);

                    // Sets the text that the label displays: [Label Name].SetText(text: string)
                    text.SetText("TAS Robotics");

                    // Keeps the image and text while the gamepad is unplugged
                    while (gamepad.GetConnectionStatus() == UsbDeviceConnection.NotConnected)
                    {
                    }
                }
                System.Threading.Thread.Sleep(100);
            }
        }
コード例 #17
0
        public static void Main()
        {
            /* Hardware */
            TalonSRX _talon = new TalonSRX(1);

            /** Use a USB gamepad plugged into the HERO */
            GameController _gamepad = new GameController(UsbHostDevice.GetInstance());

            /* String for output */
            StringBuilder _sb = new StringBuilder();

            /** hold bottom left shoulder button to enable motors */
            const uint kEnableButton = 7;

            /* Loop tracker for prints */
            int _loops = 0;

            /* Initialization */
            /* Factory Default all hardware to prevent unexpected behaviour */
            _talon.ConfigFactoryDefault();

            /* Config sensor used for Primary PID [Velocity] */
            _talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
                                                Constants.kPIDLoopIdx,
                                                Constants.kTimeoutMs);

            /**
             * Phase sensor accordingly.
             * Positive Sensor Reading should match Green (blinking) Leds on Talon
             */
            _talon.SetSensorPhase(false);

            /* Config the peak and nominal outputs */
            _talon.ConfigNominalOutputForward(0, Constants.kTimeoutMs);
            _talon.ConfigNominalOutputReverse(0, Constants.kTimeoutMs);
            _talon.ConfigPeakOutputForward(1, Constants.kTimeoutMs);
            _talon.ConfigPeakOutputReverse(-1, Constants.kTimeoutMs);

            /* Config the Velocity closed loop gains in slot0 */
            _talon.Config_kF(Constants.kPIDLoopIdx, Constants.kF, Constants.kTimeoutMs);
            _talon.Config_kP(Constants.kPIDLoopIdx, Constants.kP, Constants.kTimeoutMs);
            _talon.Config_kI(Constants.kPIDLoopIdx, Constants.kI, Constants.kTimeoutMs);
            _talon.Config_kD(Constants.kPIDLoopIdx, Constants.kD, Constants.kTimeoutMs);

            /* loop forever */
            while (true)
            {
                /* Get gamepad axis */
                double leftYstick = -1 * _gamepad.GetAxis(1);

                /* Get Talon/Victor's current output percentage */
                double motorOutput = _talon.GetMotorOutputPercent();

                /* Prepare line to print */
                _sb.Append("\tout:");
                /* Cast to int to remove decimal places */
                _sb.Append((int)(motorOutput * 100));
                _sb.Append("%");                    // Percent

                _sb.Append("\tspd:");
                _sb.Append(_talon.GetSelectedSensorVelocity(Constants.kPIDLoopIdx));
                _sb.Append("u");                    // Native units

                /**
                 * When button 1 is held, start and run Velocity Closed loop.
                 * Velocity Closed Loop is controlled by joystick position x2000 RPM, [-2000, 2000] RPM
                 */
                if (_gamepad.GetButton(1))
                {
                    /* Velocity Closed Loop */

                    /**
                     * Convert 2000 RPM to units / 100ms.
                     * 4096 Units/Rev * 2000 RPM / 600 100ms/min in either direction:
                     * velocity setpoint is in units/100ms
                     */
                    double targetVelocity_UnitsPer100ms = leftYstick * 2000.0 * 4096 / 600;
                    /* 2000 RPM in either direction */
                    _talon.Set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);

                    /* Append more signals to print when in speed mode. */
                    _sb.Append("\terr:");
                    _sb.Append(_talon.GetClosedLoopError(Constants.kPIDLoopIdx));
                    _sb.Append("\ttrg:");
                    _sb.Append(targetVelocity_UnitsPer100ms);
                }
                else
                {
                    /* Percent Output */

                    _talon.Set(ControlMode.PercentOutput, leftYstick);
                }

                /* Print built string every 10 loops */
                if (++_loops >= 10)
                {
                    _loops = 0;
                    Debug.Print(_sb.ToString());
                }
                /* Reset built string */
                _sb.Clear();

                //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*/
                    Watchdog.Feed();
                }

                /* wait a bit */
                System.Threading.Thread.Sleep(20);
            }
        }
コード例 #18
0
ファイル: Robot.cs プロジェクト: umrest/HERO-Code-2018
 public XboxController()
 {
     this.controller = new GameController(UsbHostDevice.GetInstance());
 }