Exemple #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 percent output mode */
            if (_btns[5] && !_btnsLast[5])
            {
                /* disable MP to clear IsLast */
                _talon.Set(ControlMode.MotionProfile, 0);
                Watchdog.Feed();
                Thread.Sleep(10);
                /* buffer new pts in HERO */
                TrajectoryPoint point = new TrajectoryPoint();
                _talon.ClearMotionProfileHasUnderrun();
                _talon.ClearMotionProfileTrajectories();
                for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
                {
                    point.position           = (float)MotionProfile.Points[i][0] * kTicksPerRotation;       //convert  from rotations to sensor units
                    point.velocity           = (float)MotionProfile.Points[i][1] * kTicksPerRotation / 600; //convert from RPM to sensor units per 100 ms.
                    point.headingDeg         = 0;                                                           //not used in this example
                    point.isLastPoint        = (i + 1 == MotionProfile.kNumPoints) ? true : false;
                    point.zeroPos            = (i == 0) ? true : false;
                    point.profileSlotSelect0 = 0;
                    point.profileSlotSelect1 = 0; //not used in this example
                    point.timeDur            = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
                    _talon.PushMotionProfileTrajectory(point);
                }
                /* send the first few pts to Talon */
                for (int i = 0; i < 5; ++i)
                {
                    Watchdog.Feed();
                    Thread.Sleep(10);
                    _talon.ProcessMotionProfileBuffer();
                }
                /*start MP */
                _talon.Set(ControlMode.MotionProfile, 1);
            }
            else if (_btns[7] && !_btnsLast[7])
            {
                _talon.Set(ControlMode.PercentOutput, 0);
            }

            /* if not in motion profile mode, update output percent */
            if (_talon.GetControlMode() != ControlMode.MotionProfile)
            {
                _talon.Set(ControlMode.PercentOutput, y);
            }

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