def robotInit(self):
""" Initalizes all subsystems and user controls. """
if not self.bareBones:
# Set up subsystems
subsystems.initialize()
# Set up user controls
oi.initialize()
if self.debug:
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
def _create_smartdashboard_buttons(self):
self._auto_program_chooser = SendableChooser()
#self._auto_program_chooser.addDefault("Cross Line", 1)
#self._auto_program_chooser.addObject("Hang Gear", 2)
self._auto_program_chooser.addObject("Do Nothing", 3)
SmartDashboard.putData("Autonomous", self._auto_program_chooser)
self._starting_chooser = SendableChooser()
self._starting_chooser.addDefault("Left", 1)
self._starting_chooser.addObject("Center", 2)
self._starting_chooser.addObject("Right", 3)
SmartDashboard.putData("Starting_Position", self._starting_chooser)
def initialize(self):
if self.initialized:
return
# initializing auto chooser
self.preferredElement = wpilib.SendableChooser()
self.preferredElement.addObject('crossLine', 0)
self.preferredElement.addObject('switch', 1)
self.preferredElement.addObject('scale', 2)
self.startingPosition = wpilib.SendableChooser()
self.startingPosition.addObject('left', 0)
self.startingPosition.addObject('middle', 1)
self.startingPosition.addObject('right', 2)
SmartDashboard.putData("Preferred Element", self.preferredElement)
SmartDashboard.putData("Starting Position", self.startingPosition)
print("AutoManager: Initialized")
self.initialized = True
def __init__(self, robot):
self.robot = robot
# update OI with logs
Event_Manager.add_listeners({
'auto.periodic' : self.log,
'teleop.periodic' : self.log
})
# notify listeners when SmartDashboard is updated
sd = NetworkTable.getTable('SmartDashboard')
sd.addTableListener(self.dashboard_listener, True)
# add autonomous mode chooser to smartdashboard
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING)
self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD)
self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT)
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# joystick events
self.joystick = Joystick(0)
l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER)
l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER)
r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER)
r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER)
btn_one = Joystick_Button(self.joystick, 1)
btn_two = Joystick_Button(self.joystick, 2)
# update the joysick axis periodically
Event_Manager.add_listener('teleop.periodic', self.update_axis)
l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed'))
l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed'))
l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed'))
l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed'))
l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released'))
l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released'))
r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed'))
btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed'))
btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
def setupDashboardControls():
# Only need to set up dashbard drive controls once
global modeChooser
if modeChooser == None:
SmartDashboard.putBoolean("Quick Turn", False)
SmartDashboard.putBoolean("Square Inputs", True)
SmartDashboard.putNumber("Fixed Left", 0.4)
SmartDashboard.putNumber("Fixed Right", 0.4)
SmartDashboard.putNumber("Rotation Gain", 0.5)
SmartDashboard.putNumber("Slow Gain", 0.5)
mc = SendableChooser()
mc.addDefault("Arcade", kModeArcade)
mc.addOption("Tank", kModeTank)
mc.addOption("Curvature", kModeCurvature)
mc.addDefault("Indexed Arcade", kModeIndexedArcade)
mc.addDefault("Indexed Tank", kModeIndexedTank)
mc.addOption("Fixed", kModeFixed)
SmartDashboard.putData("Drive Mode", mc)
modeChooser = mc
def __init__(self, default=ledstrip.LEDStrip.Command.RED):
'''
Constructor
'''
self.command = default
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("Red", ledstrip.LEDStrip.Command.RED)
self.chooser.addObject("Blue", ledstrip.LEDStrip.Command.BLUE)
self.chooser.addObject("Green", ledstrip.LEDStrip.Command.GREEN)
self.chooser.addObject("Rainbow", ledstrip.LEDStrip.Command.RAINBOW)
self.chooser.addObject("Theater Blue",
ledstrip.LEDStrip.Command.THEATER_BLUE)
self.chooser.addObject("Theater Red",
ledstrip.LEDStrip.Command.THEATER_RED)
self.chooser.addObject("Theater Rainbow",
ledstrip.LEDStrip.Command.THEATER_RAINBOW)
self.chooser.addObject("Off", ledstrip.LEDStrip.Command.OFF)
SmartDashboard.putData("LEDs", self.chooser)
self.r = 0
self.g = 0
self.b = 0
def robotInit(self):
""" Initalizes all subsystems and user controls. """
if self.numSubsystems > 0:
# Create drive subsystems
pwmOfs: int = 0
if self.enableDrive:
leftMotors = SpeedControllerGroup(wpilib.VictorSP(0), wpilib.VictorSP(1))
rightMotors = SpeedControllerGroup(wpilib.VictorSP(2), wpilib.VictorSP(3))
gamePad: GenericHID = XboxController(0)
drive: Drive = Drive(99, leftMotors, rightMotors)
drive.setDefaultCommand(DriveArcade(drive, leftMotors, rightMotors, gamePad))
pwmOfs += 4
for i in range(0, self.numSubsystems):
pwm: int = pwmOfs + i * 2
leftMotor: SpeedController = wpilib.VictorSP(pwm)
rightMotor: SpeedController = wpilib.VictorSP(pwm + 1)
drive = Drive(i, leftMotor, rightMotor)
SmartDashboard.putData("Forward " + str(i), DrivePower(drive, 0.2, 0.2))
SmartDashboard.putData("Backward " + str(i), DrivePower(drive, -0.2, -0.2))
# Add command to dashboard to track time for one periodic pass
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
def __init__(self, robot):
self.robot = robot
# Controllers
# Xbox
self.xbox_controller_1 = controls.xbox_controller.Xbox_Controller(0)
self.xbox_controller_2 = controls.xbox_controller.Xbox_Controller(1)
## COMMANDS ##
##Controller 1 Commands
# DRIVE COMMANDS
self.drive_command = commands.drivetrain.Drive_With_Tank_Values(
self.robot,
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_RIGHT_X,
inverted=INVERT_XBOX_RIGHT_X),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_Y,
inverted=INVERT_XBOX_LEFT_Y),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_X,
inverted=INVERT_XBOX_LEFT_X),
)
self.robot.drive.setDefaultCommand(self.drive_command)
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Do Nothing',
commands.autonomous.Do_Nothing(self.robot))
self.auto_choose.addDefault(
'Cross Baseline', commands.autonomous.Cross_Baseline(self.robot))
self.auto_choose.addObject(
'Center Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_CENTER_GEAR))
self.auto_choose.addDefault(
'Center Gear No Camera',
commands.autonomous.Center_Gear_Without_Camera(self.robot))
self.auto_choose.addObject(
'Right Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_RIGHT_GEAR))
self.auto_choose.addObject(
'Right Gear And Shoot Red',
commands.autonomous.Move_To_Gear_And_Shoot(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Left Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_LEFT_GEAR))
self.auto_choose.addObject(
'Left Gear And Shoot Blue',
commands.autonomous.Move_To_Gear_And_Shoot(
self.robot, const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Red',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Blue',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Start',
commands.autonomous.Shoot_From_Start(self.robot))
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# Toggle Drive State
self.button_drive_shift = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_LEFT_TRIGGER)
self.button_drive_shift.whenPressed(
commands.drivetrain.Toggle_Drive_State(self.robot))
# Toggle drivetrain gear
if not const.DEMO_MODE:
self.button_gear_shift = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_LEFT_BUMPER)
self.button_gear_shift.whenPressed(
commands.drivetrain.Toggle_Gear_State(self.robot))
# Slow speed for driving in Y direction so that we slow down before the gear peg
self.button_slow_driving = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_RIGHT_TRIGGER)
self.button_slow_driving.whenPressed(
commands.drivetrain.Set_Sensitivity_Low(self.robot))
self.button_slow_driving.whenReleased(
commands.drivetrain.Set_Sensitivity_High(self.robot))
# Drives backwards small distance, to align better for gear feeding
self.button_drive_gear_feeder_reverse = Xbox_Button(
self.xbox_controller_1, XBOX_BTN_X)
self.button_drive_gear_feeder_reverse.whenPressed(
commands.drivetrain.Back_Up(self.robot))
if not const.DEMO_MODE:
# For testing
self.button_rotate_to_boiler = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_Y)
self.button_rotate_to_boiler.whenPressed(
commands.drivetrain.Rotate_To_Boiler(self.robot))
# For testing
self.rotate = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_RIGHT_BUMPER)
self.rotate.whenPressed(
commands.drivetrain.Rotate_To_Angle(self.robot, -52))
self.rotate_to_gear = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_A)
self.rotate_to_gear.whenPressed(
commands.drivetrain.Rotate_To_Gear(self.robot))
## Controller 2 Commands
# Run Shooter
if const.DEMO_MODE:
# Lower top speed on shooter when in demo mode
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
else:
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel_Full_Speed(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
# Run Agitator
self.button_run_agitator = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_LEFT_TRIGGER)
self.button_run_agitator.whenPressed(
commands.agitator.Run_Agitator(self.robot, 0.8))
self.button_run_agitator.whenReleased(
commands.agitator.Stop_Agitator(self.robot))
# FEEDER COMMANDS
# Toggles feeder on/off when pressed
self.button_run_feeder_forward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_RIGHT_BUMPER)
self.button_run_feeder_forward.whenPressed(
commands.feeder.Run_Feeder(self.robot, 1.0))
self.button_run_feeder_forward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
self.button_run_feeder_backward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_LEFT_BUMPER)
self.button_run_feeder_backward.whenPressed(
commands.feeder.Run_Feeder(self.robot, -1.0))
self.button_run_feeder_backward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
if not const.DEMO_MODE:
# GILLOTUINNEE
self.button_toggle_gear_lexan = Xbox_Button(
self.xbox_controller_2, XBOX_BTN_Y)
self.button_toggle_gear_lexan.whenPressed(
commands.gear_lexan.Toggle_Gear_Lexan_State(self.robot))
#self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_OPEN ) )
#self.button_toggle_gear_lexan.whenReleased( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED) )
# Release gear
self.button_toggle_gear_release = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_A)
self.button_toggle_gear_release.whenPressed(
commands.gear_claw.Open_Container(self.robot))
self.button_toggle_gear_release.whenReleased(
commands.gear_claw.Close_Container(self.robot))
# Toggle funnel
self.button_toggle_gear_funnel = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_B)
self.button_toggle_gear_funnel.whenPressed(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
self.button_toggle_gear_funnel.whenPressed(
commands.gear_lexan.Set_Gear_Lexan_State(
self.robot, const.ID_GEAR_LEXAN_CLOSED))
self.button_toggle_gear_funnel.whenReleased(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
# CLIMBER COMMANDS
self.button_run_climber_up = Joystick_POV(
self.xbox_controller_2, controls.joystick_pov.JOY_POV_UP)
self.button_run_climber_up.whenPressed(
commands.climber.Run_Climber(self.robot, 1.0))
self.button_run_climber_up.whenReleased(
commands.climber.Stop_Climber(self.robot))
#self.button_run_climber_down = Joystick_POV( self.xbox_controller_1, controls.joystick_pov.JOY_POV_DOWN )
#self.button_run_climber_down.whenPressed( commands.climber.Run_Climber( self.robot, -0.2 ) )
#self.button_run_climber_down.whenReleased( commands.climber.Stop_Climber( self.robot ) )
#self.button_change_gear_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_BACK )
#self.button_change_gear_camera_type.whenPressed( commands.gear_camera.Toggle_Gear_Camera_Mode( self.robot ) )
#self.button_change_shooter_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_START )
#self.button_change_shooter_camera_type.whenPressed( commands.shooter_camera.Toggle_Shooter_Camera_Mode( self.robot ) )
#smart dashboard changing PID values
dp_trigger = SmartDashboard_Update_Trigger('Drive P: ',
self.robot.drive.default_kP)
dp_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(p=dp_trigger.
get_key_value())))
di_trigger = SmartDashboard_Update_Trigger('Drive I: ',
self.robot.drive.default_kI)
di_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(i=di_trigger.
get_key_value())))
dd_trigger = SmartDashboard_Update_Trigger('Drive D: ',
self.robot.drive.default_kD)
dd_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(d=dd_trigger.
get_key_value())))
#smart dashboard changing Shooter PID values
sp_trigger = SmartDashboard_Update_Trigger(
'Shooter P: ', self.robot.shooter.default_kP)
sp_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
p=sp_trigger.get_key_value())))
si_trigger = SmartDashboard_Update_Trigger(
'Shooter I: ', self.robot.shooter.default_kI)
si_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
i=si_trigger.get_key_value())))
sd_trigger = SmartDashboard_Update_Trigger(
'Shooter D: ', self.robot.shooter.default_kD)
sd_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
d=sd_trigger.get_key_value())))
sf_trigger = SmartDashboard_Update_Trigger(
'Shooter F: ', self.robot.shooter.default_kF)
sf_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
f=sf_trigger.get_key_value())))
enc_trigger = SmartDashboard_Update_Trigger(
'ENC Autocorrect constant ',
const.DRIVE_CORRECTION_PROPORTION_FORWARD_ENC)
enc_trigger.whenActive(
Command_Call(
lambda: const.update_enc_const(enc_trigger.get_key_value())))
drive_corr_trigger = SmartDashboard_Update_Trigger(
'Drive Correction Enbaled ', const.DRIVE_CORRECTION_ENABLED)
drive_corr_trigger.whenActive(
Command_Call(lambda: const.update_auto_correct(drive_corr_trigger.
get_key_value())))
shooter_trigger = SmartDashboard_Update_Trigger(
'Shooter Speed ', const.SHOOTER_SPEED)
shooter_trigger.whenActive(
Command_Call(lambda: const.update_shooter_speed(shooter_trigger.
get_key_value())))
# Trigger for changing cameras exposure
cam_exp_trigger = SmartDashboard_Update_Trigger(
'Cam Exposure ', const.DEFAULT_CAMERA_EXPOSURE)
cam_exp_trigger.whenActive(
Command_Call(lambda: self._update_cameras_exposure(
cam_exp_trigger.get_key_value())))
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
#===========================================================================================
# if LOGGER_LEVEL == logging.INFO:
# self.smartDashboard.putNumber("rVelocity",
# self.driveTrain.getRightVelocity())
# self.smartDashboard.putNumber("lVelocity",
# self.driveTrain.getLeftVelocity())
# self.smartDashboard.putNumber("rVoltage",
# self.driveTrain.getRightVoltage())
# self.smartDashboard.putNumber("lVoltage",
# self.driveTrain.getLeftVoltage())
# self.smartDashboard.putNumber("TimeStamp",
# self.timer.get())
#===========================================================================================
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
self.startingPosition = self.startSpotChooser.getSelected()
self.scaleDisable = self.scaleDisableChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
logger.info("Game Data: %s" % (self.gameData))
logger.info("Starting Position %s" % (self.startingPosition))
logger.info("Scale Enable %s" % (self.scaleDisable))
self.autonCommand = self.chooserOptions[self.startingPosition][self.gameData[0]][self.gameData[1]][self.scaleDisable]['command'](self)
self.autonCommand.start()
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
def createObjects(self):
#navx
self.navx = AHRS.create_spi()
self.navx.setPIDSourceType(PIDSource.PIDSourceType.kDisplacement)
#Drivetrain
self.left_talon0 = ctre.CANTalon(0)
self.left_talon1 = ctre.CANTalon(1)
self.right_talon0 = ctre.CANTalon(2)
self.right_talon1 = ctre.CANTalon(3)
#Set up talon slaves
self.left_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_talon1.set(self.left_talon0.getDeviceID())
self.right_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_talon1.set(self.right_talon0.getDeviceID())
#Set talon feedback device
self.left_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
#Set the Ticks per revolution in the talons
self.left_talon0.configEncoderCodesPerRev(1440)
self.right_talon0.configEncoderCodesPerRev(1440)
#Reverse left talon
self.left_talon0.setInverted(True)
self.right_talon0.setInverted(False)
#Climber
self.climber_motor = wpilib.Spark(0)
self.climber_2 = wpilib.Spark(1)
#Sensors
self.left_enc = encoder.Encoder(self.left_talon0)
self.right_enc = encoder.Encoder(self.right_talon0, True)
#Controls
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.climber_joystick = wpilib.Joystick(2)
self.buttons = unifiedjoystick.UnifiedJoystick(
[self.left_joystick, self.right_joystick])
self.last_button_state = False
#Bling
self.leds = ledstrip.LEDStrip()
#Autonomous Placement
self.auto_positions = wpilib.SendableChooser()
self.auto_positions.addDefault("Position 1", 1)
self.auto_positions.addObject("Position 2", 2)
self.auto_positions.addObject("Position 3", 3)
SmartDashboard.putData("auto_position", self.auto_positions)
#SD variables
SmartDashboard.putNumber("Vision/Turn", 0)
SmartDashboard.putBoolean("Reversed", True)
#LiveWindow
LiveWindow.addActuator("Drive", "Left Master Talon", self.left_talon0)
LiveWindow.addActuator("Drive", "Right Master Talon",
self.right_talon0)
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# Get the prioritized scoring element, robot starting posion, and the alliance
# scale/switch data.
self.selectedCrossFieldEnable = self.crossFieldChooser.getSelected()
self.selectedScoringElement = self.scoringElementChooser.getSelected()
self.selectedStartingPosition = self.positionChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
self.crossFieldEnable = self.selectedCrossFieldEnable.getCrossFieldEnable(
)
self.scoringElement = self.selectedScoringElement.getScoringElement()
self.startingPosition = self.selectedStartingPosition.getStartingPosition(
)
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
if self.scheduleAutonomous:
self.scheduleAutonomous = False
logger.info("Game Data: %s" % (self.gameData))
logger.info("Cross Field Enable: %s" % (self.crossFieldEnable))
logger.info("Scoring Element %s" % (self.scoringElement))
logger.info("Starting Position %s" % (self.startingPosition))
self.autonMiddleStartLeftSwitch.start()
#self.autonLeftStartLeftScale.start()
#self.autonForward.start()
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
