예제 #1
0
 ///////////////////////// Function to Stop All Motors  /////////////////////////
 static void StopAllMotors()
 {
     //  Stopping Each Individual Motor
     frontMotor.Set(0.0f);
     backRightMotor.Set(0.0f);
     backLeftMotor.Set(0.0f);
 }
예제 #2
0
 /**
  * Zero the sensor and zero the throttle.
  */
 void ZeroSensorAndThrottle()
 {
     _talon.SetPosition(0); /* start our position at zero, this example uses relative positions */
     _targetPosition = 0;
     /* zero throttle */
     _talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
     _talon.Set(0);
     Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */
 }
예제 #3
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(CTRE.TalonSrx.ControlMode.kMotionProfile);
                _talon.Set(0);
                CTRE.Watchdog.Feed();
                Thread.Sleep(10);
                /* buffer new pts in HERO */
                CTRE.TalonSrx.TrajectoryPoint point = new CTRE.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(CTRE.TalonSrx.ControlMode.kVoltage);
            }

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

            /* copy btns => btnsLast */
            System.Array.Copy(_btns, _btnsLast, _btns.Length);
        }
예제 #4
0
        /** spin in this routine forever */
        public void RunForever()
        {
            /* config our talon, don't continue until it's successful */
            int initStatus = SetupConfig(); /* configuration */

            while (initStatus != 0)
            {
                Instrument.PrintConfigError();
                initStatus = SetupConfig(); /* (re)config*/
            }
            /* robot loop */
            while (true)
            {
                /* get joystick params */
                float leftY = -1f * _gamepad.GetAxis(1);
                bool  btnTopLeftShoulder = _gamepad.GetButton(5);
                bool  btnBtmLeftShoulder = _gamepad.GetButton(7);
                Deadband(ref leftY);

                /* keep robot enabled if gamepad is connected and in 'D' mode */
                if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
                {
                    CTRE.Watchdog.Feed();
                }

                /* set the control mode based on button pressed */
                if (btnTopLeftShoulder)
                {
                    _mode = CTRE.TalonSrx.ControlMode.kPercentVbus;
                }
                if (btnBtmLeftShoulder)
                {
                    _mode = CTRE.TalonSrx.ControlMode.kMotionMagic;
                }

                /* calc the Talon output based on mode */
                if (_mode == CTRE.TalonSrx.ControlMode.kPercentVbus)
                {
                    float output = leftY; // [-1, +1] percent duty cycle
                    _talon.SetControlMode(_mode);
                    _talon.Set(output);
                }
                else if (_mode == CTRE.TalonSrx.ControlMode.kMotionMagic)
                {
                    float servoToRotation = leftY * 10;// [-10, +10] rotations
                    _talon.SetControlMode(_mode);
                    _talon.Set(servoToRotation);
                }
                /* instrumentation */
                Instrument.Process(_talon);

                /* wait a bit */
                System.Threading.Thread.Sleep(5);
            }
        }
예제 #5
0
        static void Drive()
        {
            if (null == _gamepad)
            {
                _gamepad = new CTRE.Gamepad(CTRE.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(leftThrot);
            leftSlave.Set(leftThrot);
            right.Set(-rightThrot);
            rightSlave.Set(-rightThrot);

            stringBuilder.Append("\t");
            stringBuilder.Append(x);
            stringBuilder.Append("\t");
            stringBuilder.Append(y);
            stringBuilder.Append("\t");
            stringBuilder.Append(twist);
        }
예제 #6
0
        static void Drive()
        {
            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(leftThrot);
            leftSlave.Set(leftThrot);
            right.Set(-rightThrot);
            rightSlave.Set(-rightThrot);

            stringBuilder.Append("\t");
            stringBuilder.Append(x);
            stringBuilder.Append("\t");
            stringBuilder.Append(y);
            stringBuilder.Append("\t");
            stringBuilder.Append(twist);
        }
예제 #7
0
        //I think this is where the magic happens and the motors move
        private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
        {
            //For each setpointData, for the talon that matches it set each mode and setpoint based on the list information
            for (int i = 0; i < setpointDataList.Count; i++)
            {
                try
                {
                    SetpointData setpointData = (SetpointData)setpointDataList[i];
                    float        setpointVal  = (float)(setpointData.getSetpoint());

                    CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
                    if (talon.GetControlMode() != setpointData.getMode())
                    {
                        talon.SetControlMode(setpointData.getMode());
                        //Debug.Print(setpointData.getMode().ToString());
                    }
                    if (talon.GetSetpoint() != setpointVal)
                    {
                        talon.Set(setpointVal);
                        Debug.Print("Setting it to value = " + setpointVal.ToString());
                        //Debug.Print(setpointVal.ToString());
                    }
                }
                catch (ArgumentOutOfRangeException ex)
                {
                    Debug.Print(ex.ToString());
                }
            }
        }
예제 #8
0
        public static void Main()
        {
            /* create a gamepad object */
            CTRE.Gamepad myGamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
            /* create a talon, the Talon Device ID in HERO LifeBoat is zero */
            CTRE.TalonSrx myTalon = new CTRE.TalonSrx(0);
            /* simple counter to print and watch using the debugger */
            int counter = 0;

            /* loop forever */
            while (true)
            {
                /* added inside the while loop */
                if (myGamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
                {
                    /* print the axis value */
                    Debug.Print("axis:" + myGamepad.GetAxis(1));
                    /* pass axis value to talon */
                    myTalon.Set(myGamepad.GetAxis(1));
                    /* allow motor control */
                    CTRE.Watchdog.Feed();
                }
                /* increment counter */
                ++counter; /* try to land a breakpoint here */
                /* wait a bit */
                System.Threading.Thread.Sleep(10);
            }
        }
예제 #9
0
        public override Int32 Run(ZSDK.Gamepad gamepad)
        {
            Single  LeftVal = 0, RightVal = 0;
            Boolean Slow = true;

            ReadGamepad(gamepad, ref LeftVal, ref RightVal, ref Slow);

#if DEBUG
            Debug.Print(ToString() + " [RAW] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif

            /* Apply RPM conversion to values. */
            if (USE_SPEED_MODE)
            {
                LeftVal  *= MAX_RPM;
                RightVal *= MAX_RPM;
            }

            /* Drive in slow mode. */
            if (Slow)
            {
                LeftVal  *= SLOW_MULT;
                RightVal *= SLOW_MULT;
            }

#if DEBUG
            Debug.Print(ToString() + " [SET] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif

            Right.Set(RightVal);
            Left.Set(LeftVal);
            return(0);
        }
예제 #10
0
        static void ArcadeDrive(float moveValue, float rotateValue)
        {
            float leftMotorSpeed;
            float rightMotorSpeed;

            rotateValue = Limit(rotateValue);
            moveValue   = Limit(moveValue);

            // Square values for smoothness
            if (rotateValue >= 0.0f)
            {
                rotateValue = rotateValue * rotateValue;
            }
            else
            {
                rotateValue = -(rotateValue * rotateValue);
            }
            if (moveValue >= 0.0f)
            {
                moveValue = moveValue * moveValue;
            }
            else
            {
                moveValue = -(moveValue * moveValue);
            }


            if (rotateValue > 0.0)
            {
                if (moveValue > 0.0)
                {
                    leftMotorSpeed  = rotateValue - moveValue;
                    rightMotorSpeed = (float)System.Math.Max(rotateValue, moveValue);
                }
                else
                {
                    leftMotorSpeed  = (float)System.Math.Max(rotateValue, -moveValue);
                    rightMotorSpeed = rotateValue + moveValue;
                }
            }
            else
            {
                if (moveValue > 0.0)
                {
                    leftMotorSpeed  = -(float)System.Math.Max(-rotateValue, moveValue);
                    rightMotorSpeed = rotateValue + moveValue;
                }
                else
                {
                    leftMotorSpeed  = rotateValue - moveValue;
                    rightMotorSpeed = -(float)System.Math.Max(-rotateValue, -moveValue);
                }
            }

            //left.Set(leftMotorSpeed);
            //leftSlave.Set(leftMotorSpeed);
            right.Set(rightMotorSpeed);
            rightSlave.Set(rightMotorSpeed);
        }
예제 #11
0
파일: Program.cs 프로젝트: schreiaj/HeroBot
        public static void CheesyDrive(float throttle, float steer)
        {
            float coeff             = 1.0f;
            float steeringComponent = steer * TURNING_CONSTANT;
            float throttleComponent = throttle * coeff;

            leftDrive.Set(throttleComponent + steeringComponent);
            rightDrive.Set(throttleComponent - steeringComponent);
        }
예제 #12
0
        public static void Main()
        {
            CTRE.Gamepad _gamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
            //0 - left x
            //1 - left y
            //2 - right x
            //
            //5 - LB
            //6 - RB

            ledSpike.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
            ledSpike.SetCurrentLimit(1);

            while (true)
            {
                float y    = -_gamepad.GetAxis(1);
                float turn = _gamepad.GetAxis(2);

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

                if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
                {
                    if (_gamepad.GetButton(5))
                    {
                        ArcadeDrive(y, turn);
                        /* feed watchdog to keep Talon's enabled */
                        CTRE.Watchdog.Feed();
                    }
                    if (_gamepad.GetButton(6))
                    {
                        ledSpike.Set(3.5f);
                        CTRE.Watchdog.Feed();
                    }
                    else
                    {
                        ledSpike.Set(0);
                    }
                }

                /* run this task every 10ms */
                Thread.Sleep(10);
            }
        }
예제 #13
0
        static void Drive()
        {
            float y     = _gamepad.GetAxis(1);
            float twist = _gamepad.GetAxis(4);

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

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

            left1.Set(leftThrot);
            right1.Set(-rightThrot);

            stringBuilder.Append("\t");
            stringBuilder.Append(y);
            stringBuilder.Append("\t");
            stringBuilder.Append(twist);
        }
예제 #14
0
 private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
 {
     for (int i = 0; i < setpointDataList.Count; i++)
     {
         SetpointData  setpointData = (SetpointData)setpointDataList[i];
         float         setpointVal  = (float)(setpointData.getSetpoint());
         CTRE.TalonSrx talon        = (CTRE.TalonSrx)talons[i];
         if (talon.GetControlMode() != setpointData.getMode())
         {
             talon.SetControlMode(setpointData.getMode());
             Debug.Print(setpointData.getMode().ToString());
         }
         if (talon.GetSetpoint() != setpointVal)
         {
             talon.Set(setpointVal);
             Debug.Print(setpointVal.ToString());
         }
     }
 }
예제 #15
0
        static void Run()
        {
            float x = _gamepad.GetAxis(1);

            Deadband(ref x);

            //Set the Maximum Current Limit for the Talon (in Amps)
            talon.SetCurrentLimit(10);
            //Enable the Current Limiting Feature.
            talon.EnableCurrentLimit(true);

            talon.Set(x);

            float current = talon.GetOutputCurrent();

            stringBuilder.Append("\t");
            stringBuilder.Append(current);
            stringBuilder.Append("\t");
        }
예제 #16
0
        static void Drive()
        {
            if (null == _gamepad)
            {
                _gamepad = new CTRE.Gamepad(CTRE.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(leftFrnt_throt);
            leftRear.Set(leftRear_throt);
            rghtFrnt.Set(rghtFrnt_throt);
            rghtRear.Set(rghtRear_throt);
        }
예제 #17
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). */
            CTRE.UsbHostDevice.EnableXInputDevices();

            while (true)
            {
                if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
                {
                    CTRE.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((_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;
            }
        }
예제 #18
0
        public override Int32 Run(ZSDK.Gamepad gamepad)
        {
            Boolean Fire = false;

            if (gamepad is ZSDK.LogitechGamepad)
            {
                if ((gamepad as ZSDK.LogitechGamepad).Button_A)
                {
                    Fire = true; Mode = FireMode.Normal;
                }
                else if ((gamepad as ZSDK.LogitechGamepad).Button_Y)
                {
                    Fire = true; Mode = FireMode.NoReset;
                }
            }
            else
            {
                if (gamepad.Buttons[0])
                {
                    Fire = true; Mode = FireMode.Normal;
                }
                else if (gamepad.Buttons[1])
                {
                    Fire = true; Mode = FireMode.NoReset;
                }
            }

            if (!Winch.GetForwardLimitOK() || !Winch.GetReverseLimitOK())
            {
                Winch.Set(0);
            }
            if (!Dog.GetForwardLimitOK() || !Dog.GetReverseLimitOK())
            {
                Dog.Set(0);
            }

#if DEBUG
            Debug.Print(ToString() + " [STATE] " + State.ToString());
#endif
            switch (State)
            {
            default:
            case BallLaunchState.Unknown:
                // State 0
                // Assume we are started Released
                Winch.Set(0);
                Dog.Set(0);
                State = BallLaunchState.Released;
                break;

            case BallLaunchState.Loaded:
                // State 1
                Winch.Set(0);
                Dog.Set(0);
                if (Fire)
                {
                    // Run Dog TalonSRX to fire.
                    Dog.Set(DOG_FIRE_DIRECTION);
                    State = BallLaunchState.Releasing;
                }
                break;

            case BallLaunchState.Loading:
                // State 2
                // Start the winch.
                // Check for winch limit switch.
                Boolean WinchLimitOK = Winch.GetReverseLimitOK();
                if (!WinchLimitOK)
                {
                    Winch.Set(0);
                    State = BallLaunchState.Loaded;
                }
                else
                {
                    Boolean WinchStart = DateTime.Now.Subtract(WinchStartTime).Ticks <= WINCH_START_WAIT;
                    Single  WinchSpeed = WinchStart ? WINCH_DIRECTION_START : WINCH_DIRECTION_END;
#if DEBUG
                    Debug.Print(ToString() + " [WINCH SPEED] " + WinchSpeed.ToString());
#endif
                    Winch.Set(WinchSpeed);
                }
                break;

            case BallLaunchState.Released:
                // State 3
                // Reset the Dog Gear.
                // Check for dog limit switch.
                Boolean DogLimitOK = Dog.GetReverseLimitOK();
                if (!DogLimitOK)
                {
                    Dog.Set(0);
                    switch (Mode)
                    {
                    default:
                    case FireMode.Normal:
                        State = BallLaunchState.Loading;
                        break;

                    case FireMode.NoReset:
                        State = BallLaunchState.Loaded;
                        break;
                    }
                    WinchStartTime = DateTime.Now;
                }
                else
                {
                    Dog.Set(DOG_RESET_DIRECTION);
                }
                break;

            case BallLaunchState.Releasing:
                // State 4
                // Wait for a complete fire.

                Boolean DogFireLimitOK = Dog.GetForwardLimitOK();
                if (!DogFireLimitOK)
                {
                    FireTime = DateTime.Now;
                    State    = BallLaunchState.Reseting;
                }
                break;

            case BallLaunchState.Reseting:
                Int64 fire_ticks = DateTime.Now.Subtract(FireTime).Ticks;
                if (fire_ticks >= FIRE_WAIT)
                {
                    State = BallLaunchState.Released;
                }
                break;
            }
            return(0);
        }