Exemplo n.º 1
0
        void Drive()
        {
            FillBtns(ref _btns);
            float y = -1 * _gamepad.GetAxis(1);

            Deadband(ref y);

            _talon.ProcessMotionProfileBuffer();

            /* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */
            if (_btns[5] && !_btnsLast[5])
            {
                /* disable MP to clear IsLast */
                _talon.SetControlMode(ControlMode.kMotionProfile);
                _talon.Set(0);
                CTRE.Watchdog.Feed();
                Thread.Sleep(10);
                /* buffer new pts in HERO */
                TalonSrx.TrajectoryPoint point = new TalonSrx.TrajectoryPoint();
                _talon.ClearMotionProfileHasUnderrun();
                _talon.ClearMotionProfileTrajectories();
                for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
                {
                    point.position          = (float)MotionProfile.Points[i][0];
                    point.velocity          = (float)MotionProfile.Points[i][1];
                    point.timeDurMs         = MotionProfile.kDurationMs;
                    point.isLastPoint       = (i + 1 == MotionProfile.kNumPoints) ? true : false;
                    point.zeroPos           = (i == 0) ? true : false;
                    point.velocityOnly      = false;
                    point.profileSlotSelect = 0;
                    _talon.PushMotionProfileTrajectory(point);
                }
                /* send the first few pts to Talon */
                for (int i = 0; i < 5; ++i)
                {
                    CTRE.Watchdog.Feed();
                    Thread.Sleep(10);
                    _talon.ProcessMotionProfileBuffer();
                }
                /*start MP */
                _talon.Set(1);
            }
            else if (_btns[7] && !_btnsLast[7])
            {
                _talon.SetControlMode(ControlMode.kVoltage);
            }

            /* if in voltage mode, update output voltage */
            if (_talon.GetControlMode() == ControlMode.kVoltage)
            {
                _talon.Set(14.0f * y);
            }

            /* copy btns => btnsLast */
            System.Array.Copy(_btns, _btnsLast, _btns.Length);
        }