robot-sim-test-02.py

#!/usr/bin/env python
import wpilib
from wpilib.drive import DifferentialDrive

DEBUG = True
#DEBUG = False

class MyRobot(wpilib.TimedRobot):
    """Main robot class"""

    def robotInit(self):
        """Robot-wide initialization code should go here"""

        self.fleft_motor = wpilib.Spark(0)
        self.rleft_motor = wpilib.Spark(1)
        self.fright_motor = wpilib.Spark(2)
        self.rright_motor = wpilib.Spark(3)

        self.left_motor = wpilib.SpeedControllerGroup(
            self.fleft_motor, self.rleft_motor
            )
        self.right_motor = wpilib.SpeedControllerGroup(
            self.fright_motor, self.rright_motor
            )

        self.myRobot = DifferentialDrive(self.left_motor, self.right_motor)
        self.myRobot.setExpiration(0.1)

        self.gamepad = wpilib.Joystick(0)

        # init empty gamepad axis dict (so can fill during debugging)
        self.gp_axis = {}

    def rescale_trigger(self, m, t_min=None, t_max=None, r_min=None, r_max=None):
        """
        Rescale number 'm' from r-scale to t-scale
        source: https://stats.stackexchange.com/questions/281162/scale-a-number-between-a-range#answer-281164
        """
        # original range (default [-1.0, 1.0])
        r_min = -1.0 if not r_min else r_min
        r_max = 1.0 if not r_max else r_max
        # new target range (default [0.0, 1.0])
        t_min = 0.0 if not t_min else t_min
        t_max = 1.0 if not t_max else t_max

        return (((m - r_min)/(r_max - r_min))*(t_max - t_min)) + t_min

    def teleopInit(self):
        """Executed at the start of teleop mode"""
        self.myRobot.setSafetyEnabled(True)

        if not DEBUG:
            return
        print("Axis Count = {}".format(self.gamepad.getAxisCount()))
        print("Button Count = {}".format(self.gamepad.getButtonCount()))
        for i in range(0, self.gamepad.getAxisCount()):
            self.gp_axis[i] = self.gamepad.getRawAxis(i)

    def disabled(self):
        """Called when the robot is disabled"""
        while self.isDisabled():
            wpilib.Timer.delay(0.01)

    def autonomous(self):
        """Called when autonomous mode is enabled"""

        while self.isAutonomous() and self.isEnabled():
            wpilib.Timer.delay(0.01)

    def teleopPeriodic(self):
        """Run motors with tank drive"""
        """
        self.myRobot.tankDrive(
            self.gamepad.getRawAxis(1),
            self.gamepad.getRawAxis(4)
            )
        """

        """Run motors with arcade drive"""
        """
        # Option (1) -- race car config
        f = self.rescale_trigger(self.gamepad.getRawAxis(5)*-1, 0.0, 1.0)
        r = self.rescale_trigger(self.gamepad.getRawAxis(2)*-1, 0.0, -1.0)
        self.myRobot.arcadeDrive(
            f+r,                            # Speed: right trigger = forward [ ↑ ], left trigger = backward [ ↓  ]
            self.gamepad.getRawAxis(3)*-1,  # Rotation: right thumb stick [ ←  , → ]
            True                            # squareInputs: decrease sensitivity at low speeds
            )
        """
        # Option (2) -- left thumb stick controls speed
        self.myRobot.arcadeDrive(
            self.gamepad.getRawAxis(1),     # Speed: left thumb stick = forward & backward [ ↑  , ↓  ]
            self.gamepad.getRawAxis(3)*-1,  # Rotation: right thumb stick [ ←  , → ]
            True                            # squareInputs: decrease sensitivity at low speeds
            )

        # add debug output
        if not DEBUG:
            return
        for j in range(1, self.gamepad.getButtonCount()+1):
            if self.gamepad.getRawButtonPressed(j):
                print("Button {} pressed".format(j))

        for i in range(0, self.gamepad.getAxisCount()):
            cur_axis = self.gamepad.getRawAxis(i)
            if self.gp_axis[i] != cur_axis:
                self.gp_axis[i] = cur_axis
                print("Axis {} = {}".format(i, cur_axis))

if __name__ == "__main__":
    wpilib.run(MyRobot)