Skip to content

Instantly share code, notes, and snippets.

@Kovrinic
Last active January 15, 2019 00:21
Show Gist options
  • Save Kovrinic/a1b3004a35bd4684e7470bcea0ecc6c5 to your computer and use it in GitHub Desktop.
Save Kovrinic/a1b3004a35bd4684e7470bcea0ecc6c5 to your computer and use it in GitHub Desktop.
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment