def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
# TODO reset heading
pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
self.chassis.reset_odometry(
geometry.Pose2d(-3, 0, geometry.Rotation2d(
math.pi))) # the starting position on the field
self.target_estimator.reset()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.drive = Drive()
self.stick = Joystick(0)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.drive.moveToPosition(10000, 'left')
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.stick.getY() * -1
rotation = self.stick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
def testInit(self):
pass
def testPeriodic(self):
pass
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# holds data from the FMS about the field elements.
self.fielddata = None
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data -- currently removed until we add an IF statement
# self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
## get field data
#if not self.fielddata:
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
#nearswitch, scale, farswitch = list(self.fielddata)
#if self.fielddata[0] == 'R':
#self.drive.arcade(.5, .2)
#else:
#self.drive.arcade(.5, -.2)
self.drive.arcade(.5, 0)
def teleopInit(self):
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
self.cubeGrabber.grabberFunction()
def testInit(self):
pass
def testPeriodic(self):
pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kElevator = robotmap.Elevator
self.kSticks = robotmap.Sticks
self.kClimber = robotmap.Climber
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = cubeGrabber(self)
self.elevator = Elevator(self)
self.climber = Climber(self)
self.sendableChooser()
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.autonomous = Autonomous(self)
self.autonomous.reset()
self.drive.autoInit()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#self.autonomous.testMove(self.autonomous.WALL_TO_SCALE, -1, False)
#self.autonomous.testAngle(-90, -1)
#self.elevator.setElevatorPosition(self.elevator.kScale)
#self.autonomous.start()
self.autonomous.run()
#self.elevator.setElevatorPosition(-20000)
#self.autonomous.telemetry()
def teleopInit(self):
self.drive.teleInit()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = (self.dStick.getY() * -1)**3
rotation = self.dStick.getTwist()/(1.1+self.dStick.getRawAxis(3))
# self.drive.moveSpeed(speed, speed)
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
#
self.elevator.elevatorFunction()
#self.elevator.telemetry()
self.climber.climberFunction()
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
class OI:
def __init__(self):
self.reversed = True
self.arcade_drive = True
self.turbo = False
self.controls = {
"reverse": 2, # top
"arcade_switch": 3,
"turbo": 1, # trigger
"reset": 4
}
self.driver_controller = Joystick(0)
self.left_power = 0
self.right_power = 0
def get_button_config_pressed(self, controller, button_name):
return controller.getRawButtonPressed(self.controls[button_name])
def get_button_config_held(self, controller, button_name):
return controller.getRawButton(self.controls[button_name])
def curve_input(self, i):
deadzone = 0.1
if abs(i) < deadzone:
return 0
return math.pow(i, 3) / 1.25
def reset(self):
self.left_power = 0
self.right_power = 0
self.reversed = False
self.arcade_drive = True
self.turbo = False
def execute(self):
if self.get_button_config_pressed(self.driver_controller, "reverse"):
self.reversed = not self.reversed
if self.get_button_config_pressed(self.driver_controller,
"arcade_switch"):
self.arcade_drive = not self.arcade_drive
self.turbo = self.get_button_config_held(self.driver_controller,
"turbo")
self.left_power = self.driver_controller.getY()
self.right_power = self.driver_controller.getTwist()
reversed_constant = -1 if self.reversed else 1
self.left_power = self.curve_input(self.left_power * reversed_constant)
self.right_power = self.curve_input(self.right_power *
reversed_constant)
if self.turbo:
self.left_power *= 1.25
self.right_power *= 1.25
if self.get_button_config_pressed(self.driver_controller, "reset"):
self.reset()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# TODO: see examples/pacgoat/robot.py
# we need Classes defining autonomous commands to call for Forward
# and Reverse
#self.autoChooser = wpilib.SendableChooser()
#self.autoChooser.addDefault("Forward", Forward(self))
#self.autoChooser.addObject("Reverse", Reverse(self))
#wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#nearswitch, scale, farswitch = list(self.fielddata)
#
# if nearswitch == 'R':
# self.drive.arcade(.5, .2)
# else:
# self.drive.arcade(.5, -.2)
pass
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
if self.isSimulation():
self.drive.arcade(speed, rotation)
else:
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
# TODO: need something like this in commands that autonomous or teleop
# would run to make sure an exception doesn't crash the code during
# competition.
#raise ValueError("Checking to see what happens when we get an exception")
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
