def initialize_dashboard(self):
# dummy setpoints to speed up testing from the dashboard
SmartDashboard.putNumber('z_distance', 2.0)
SmartDashboard.putNumber('z_angle', 60)
self.drive_fwd_command = AutonomousDriveTimed(self.robot, timeout=1)
self.rotate_command = AutonomousRotate(self.robot,
setpoint=45,
timeout=3,
source='dashboard')
self.autonomous_test_command = AutonomousSlalom(self.robot)
self.autonomous_test_ramsete_command = AutonomousRamsete(self.robot)
self.autonomous_pid_command = AutonomousDrivePID(self.robot,
setpoint=2,
timeout=4,
source='dashboard')
# these don't work right in 2021 as of 20210131 - keep interrupting and restarting - old ocmmand structure issue?
#SmartDashboard.putData("Auto Ramsete", self.autonomous_test_ramsete_command)
#SmartDashboard.putData("Auto PID", self.autonomous_pid_command)
#SmartDashboard.putData("Auto Drive", self.drive_fwd_command )
SmartDashboard.putData('Rotate command', self.rotate_command)
# set up the dashboard chooser for the autonomous options
self.obstacle_chooser = SendableChooser()
routes = ['slalom', 'barrel', 'bounce', 'none']
for ix, position in enumerate(routes):
if ix == 3:
self.obstacle_chooser.setDefaultOption(position, position)
else:
self.obstacle_chooser.addOption(position, position)
wpilib.SmartDashboard.putData('obstacles', self.obstacle_chooser)
self.path_chooser = SendableChooser()
wpilib.SmartDashboard.putData('ramsete path', self.path_chooser)
#choices = ['loop', 'poses', 'points', 'test', 'slalom_pw0_0.75','slalom_pw1_0.75', 'slalom_pw2_0.75', 'slalom_pw0_1.25', 'slalom_pw1_1.25',
# 'slalom_pw2_1.25', 'slalom_pw1_1.80', 'barrel_pw1_0.75', 'barrel_pw1_1.25', 'bounce_pw1_0.75', 'bounce_pw1_1.25', 'student_pw0_0p75',
# 'student_pw1_0p75','student_pw0_1p25', 'student_pw1_1p25']
#choices = drive_constants.get_pathweaver_files() + ['z_loop', 'z_poses', 'z_points', 'z_test']
choices = drive_constants.get_pathweaver_paths(
simulation=self.robot.isSimulation()) + [
'z_loop', 'z_poses', 'z_points', 'z_test'
]
for ix, position in enumerate(choices):
if ix == 0:
self.path_chooser.setDefaultOption(position, position)
else:
self.path_chooser.addOption(position, position)
self.velocity_chooser = SendableChooser()
wpilib.SmartDashboard.putData('path velocity', self.velocity_chooser)
velocities = [
0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 6.0, 7.0
]
for ix, position in enumerate(velocities):
if ix == 4: # 2.5 will be the default
self.velocity_chooser.setDefaultOption(str(position), position)
else:
self.velocity_chooser.addOption(str(position), position)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
wpilib.CameraServer.launch()
self.lastAuto = False
Command.getRobot = lambda x=0: self
# Load system preferences prior to constructing
# subsystems
map.loadPreferences()
# Construct subsystems prior to constructing commands
#self.limelight = Limelight.Limelight(self) #not a subsystem
self.hatch = HatchMech.HatchMech(self)
self.hatch.initialize()
self.cargo = CargoMech.CargoMech() #not a subsystem
self.cargo.initialize()
self.climber = Climber.Climber() #not a subsystem
self.climber.initialize(self)
self.drive = Drive.Drive(self)
self.compressor = wpilib.Compressor(0)
self.timer = wpilib.Timer()
self.timer.start()
self.watch = wpilib.Watchdog(150, None)
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
[self.joystick0, self.joystick1, self.xbox] = oi.commands()
self.updateDashboardInit()
self.DriveStraight = DriveStraight()
self.DriveStraightSide = DriveStraightSide()
self.LeftCargo = LeftCargo()
self.RightCargo = RightCargo()
self.CenterCargo = CenterCargo()
self.SetSpeedDT = SetSpeedDT()
self.CenterCargoPart2 = CenterCargoPart2()
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption("Drive Straight", "DriveStraight")
self.autonChooser.addOption("Left Cargo", "LeftCargo")
self.autonChooser.addOption("Right Cargo", "RightCargo")
self.autonChooser.addOption("Do Nothing", "DoNothing")
self.autonChooser.addOption("Level 1 Center", "Level1Center")
self.autonChooser.addOption("Drive Straight Side", "DriveStraightSide")
SmartDashboard.putData("Auto mode", self.autonChooser)
def robotInit(self):
#self.smartDashboard = smart_dashboard
self.sd = NetworkTables.getTable("SmartDashboard")
self.left_motor = ctre.WPI_TalonSRX(0)
self.right_motor = ctre.WPI_TalonSRX(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.stick = wpilib.Joystick(0)
self.navx = navx.AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = PIDController(self.kP, self.kI, self.kD, period=1.0)
self.turnController = turnController
self.rotateToAngleRate = 5
smart_dashboard.putData("PID COntroller", self.turnController)
def __init__(self, robot):
self.joystick = wpilib.Joystick(0)
JoystickButton(self.joystick, 12).whenPressed(LowGoal(robot))
JoystickButton(self.joystick, 10).whenPressed(Collect(robot))
JoystickButton(self.joystick, 11).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT))
JoystickButton(self.joystick, 9).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT_NEAR))
DoubleButton(self.joystick, 2, 3).whenActive(Shoot(robot))
#SmartDashboard Buttons
SmartDashboard.putData("Drive Forward", DriveForward(robot,2.25))
SmartDashboard.putData("Drive Backward", DriveForward(robot,-2.25))
SmartDashboard.putData("Start Rollers", SetCollectionSpeed(robot,Collector.FORWARD))
SmartDashboard.putData("Stop Rollers", SetCollectionSpeed(robot,Collector.STOP))
SmartDashboard.putData("Reverse Rollers", SetCollectionSpeed(robot,Collector.REVERSE))
def __init__(self):
super().__init__()
# Since MyRobot implements all of the methods required
# to be a PIDSource and PIDOutput, it can be passed
# as the last two parameter to the PIDController object.
pc = wpilib.PIDController(0.01, 0.0, 0.0, 0.0, self, self)
# Putting the PIDController to the SmartDashboard allows
# you to experiment with the values on the dashboard
SmartDashboard.putData(pc)
self.pc = pc
# Fake sensor reading
self.sensor = 5.0
self.output = 0.0
# Create a motor controller to apply power to so we
# can see something in the simulator
self.spark = wpilib.Spark(1)
def robotInit(self):
##sd = NetworkTables.getTable('SmartDashboard')
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
#wpilib.SmartDashboard.getString("Choice", "null")
choice = SendableChooser()
choice.setDefaultOption('option0', 0)
choice.addOption('option1', 1)
choice.addOption('option2', 2)
SmartDashboard.putData("Test", choice)
self.drive.setExpiration(0.1)
self.tm = wpilib.Timer()
self.tm.start()
def robotInit(self):
self.controller = Joystick(0)
self.joystick = Joystick(1)
CameraServer.launch('vision.py:main')
self.autoChooser = SendableChooser()
self.autoChooser.addDefault("switch_scale", switch_scale)
# self.autoChooser.addObject("drive_forward", drive_straight)
SmartDashboard.putData("Autonomous Mode Chooser", self.autoChooser)
self.autoSideChooser = SendableChooser()
self.autoSideChooser.addDefault("left", "L")
self.autoSideChooser.addObject("right", "R")
self.autoSideChooser.addObject("middle", "M")
SmartDashboard.putData("Side Chooser", self.autoSideChooser)
RobotMap.driver_component.set_low_gear()
def dashboardPeriodic(self):
#self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
#self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
#self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
#self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
#self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
#self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
SmartDashboard.putNumber("Wrist Position",
self.wrist.getQuadraturePosition())
SmartDashboard.putData("PID Controller", self.cargoController)
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
SmartDashboard.putNumber("Wrist Power", self.input)
SmartDashboard.putNumber("Wrist Power2", self.input2)
SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand)
SmartDashboard.putBoolean("Wrist PinState Quad A",
self.wrist.getPinStateQuadA())
SmartDashboard.putBoolean("Wrist PinState Quad B",
self.wrist.getPinStateQuadB())
self.F = SmartDashboard.getNumber("F Gain", 0)
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
self.target_chooser = sendablechooser.SendableChooser()
self.target_chooser.setDefaultOption(TargetHeight.LOW.name,
TargetHeight.LOW)
self.target_chooser.addOption(TargetHeight.MIDDLE.name,
TargetHeight.MIDDLE)
self.target_chooser.addOption(TargetHeight.HIGH.name,
TargetHeight.HIGH)
self.left_right_chooser = sendablechooser.SendableChooser()
self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name,
RightLeft.RIGHT)
self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT)
self.hab_level_chooser = sendablechooser.SendableChooser()
self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name,
HabLevel.LEVEL1)
self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2)
SmartDashboard.putData("TargetChooser", self.target_chooser)
SmartDashboard.putData("LeftRightChooser", self.left_right_chooser)
SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser)
self.chosen_target = TargetHeight.LOW
self.left_right = RightLeft.RIGHT
self.hab_level = HabLevel.LEVEL1
def __init__(self, robot):
self.robot = robot
if not robot.debug:
self.initialize_joysitics()
else:
self.stick = wpilib.Joystick(0)
# SmartDashboard Buttons - test some autonomous commands here
SmartDashboard.putNumber("Auto Distance", 10)
SmartDashboard.putNumber("Auto Rotation", 10)
SmartDashboard.putData("Drive Forward", AutonomousDrive(robot, setpoint=None, control_type='position', timeout=6))
SmartDashboard.putData("Rotate X", AutonomousRotate(robot, setpoint=None, timeout=6))
SmartDashboard.putData("Update Pos PIDs", (UpdatePIDs(robot, factor=1, from_dashboard='position')))
SmartDashboard.putData("Update Vel PIDs", (UpdatePIDs(robot, factor=1, from_dashboard='velocity')))
def robotInit(self):
self.timer = wpilib.Timer()
wpilib.CameraServer.launch()
#Initialize limit swithces
self.autoChuteLeft = wpilib.DigitalInput(0)
#self.autoChuteStraight = wpilib.DigitalInput(3)
self.autoChuteRight = wpilib.DigitalInput(2)
self.autoChuteSafe = wpilib.DigitalInput(1)
self.compressor = wpilib.Compressor(robotmap.PCM_ID)
self.compressor.setClosedLoopControl(True)
#Initialize Controllers
self.pilot = XBox(0)
self.copilot = XBox(1)
#Initialize Subsystems
self.drive = InfiniteRechargeDrive()
self.ballChute = BallChute()
self.colorWheel = ColorWheel()
self.climb = Climb()
#Initialize Autonomous
self.autochuteLeft = AutoChuteLeft(self.drive, self.ballChute,
self.logger)
self.autochuteRight = AutoChuteRight(self.drive, self.ballChute)
self.autochuteSafe = AutoChuteSafe(self.drive, self.ballChute)
self.autochuteClear = AutoChuteClear(self.drive, self.ballChute)
self.auto_chooser = SendableChooser()
self.auto_chooser.setDefaultOption('AutochuteClear',
self.autochuteClear)
self.auto_chooser.addOption('AutochuteLeft', self.autochuteLeft)
self.auto_chooser.addOption('AutochuteRight', self.autochuteRight)
self.auto_chooser.addOption("AutochuteSafe", self.autochuteSafe)
SmartDashboard.putData("AutoChooser", self.auto_chooser)
def init(self):
self.logger.info("DeepSpaceLift::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.robot_mode = RobotMode.TEST
self.last_lift_adjust_time = 0
self.lift_adjust_timer = wpilib.Timer()
self.lift_adjust_timer.start()
self.state_timer = wpilib.Timer()
self.current_state = LiftState.LIFT_START_CONFIGURATION
self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW
self.current_lift_preset_val = self.lift_stow_position
self.lift_setpoint = self.min_lift_position
self.lift_adjust_val = 0
self.lift_talon = TalonSRX(robotmap.LIFT_CAN_ID)
'''Select and zero sensor in init() function. That way the zero position doesn't get reset every time we enable/disable robot'''
self.lift_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.setSelectedSensorPosition(0, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_pneumatic_extend = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_RAISE_SOLENOID)
self.lift_pneumatic_retract = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_LOWER_SOLENOID)
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.test_lift_pneumatic = sendablechooser.SendableChooser()
self.test_lift_pneumatic.setDefaultOption("Retract", 1)
self.test_lift_pneumatic.addOption("Extend", 2)
SmartDashboard.putData("TestLiftPneumatic", self.test_lift_pneumatic)
def init(self):
self.logger.info("DeepSpaceClaw::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.state_timer = wpilib.Timer()
self.current_state = ClawState.CLAW_STOWED
self.ball_infrared = wpilib.DigitalInput(robotmap.BALL_IR_SENSOR)
self.left_grab = ctre.TalonSRX(robotmap.CLAW_LEFT_WHEELS_CAN_ID)
self.right_grab = ctre.TalonSRX(robotmap.CLAW_RIGHT_WHEELS_CAN_ID)
self.talons = [self.left_grab, self.right_grab]
self.claw_open = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.CLAW_OPEN_SOLENOID)
self.claw_close = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.CLAW_CLOSE_SOLENOID)
self.wrist_down = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.WRIST_EXTEND_SOLENOID)
self.wrist_up = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.WRIST_RETRACT_SOLENOID)
self.claw_close.set(True)
self.claw_open.set(False)
self.wrist_down.set(False)
self.wrist_up.set(True)
self.test_wrist = sendablechooser.SendableChooser()
self.test_wrist.setDefaultOption("Retract", 1)
self.test_wrist.addOption("Extend", 2)
self.test_claw = sendablechooser.SendableChooser()
self.test_claw.setDefaultOption("Retract", 1)
self.test_claw.addOption("Extend", 2)
SmartDashboard.putData("TestWrist", self.test_wrist)
SmartDashboard.putData("TestClaw", self.test_claw)
SmartDashboard.putNumber("TestClawMotors", 0)
def __init__(self, robot):
self.joystick = wpilib.Joystick(0)
JoystickButton(self.joystick, 12).whenPressed(LowGoal(robot))
JoystickButton(self.joystick, 10).whenPressed(Collect(robot))
JoystickButton(self.joystick,
11).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT))
JoystickButton(self.joystick,
9).whenPressed(SetPivotSetpoint(robot,
Pivot.SHOOT_NEAR))
DoubleButton(self.joystick, 2, 3).whenActive(Shoot(robot))
# SmartDashboard Buttons
SmartDashboard.putData("Drive Forward", DriveForward(robot, 2.25))
SmartDashboard.putData("Drive Backward", DriveForward(robot, -2.25))
SmartDashboard.putData("Start Rollers",
SetCollectionSpeed(robot, Collector.FORWARD))
SmartDashboard.putData("Stop Rollers",
SetCollectionSpeed(robot, Collector.STOP))
SmartDashboard.putData("Reverse Rollers",
SetCollectionSpeed(robot, Collector.REVERSE))
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.auto1 = Auto1(self, self.logger)
self.auto2 = Auto2(self, self.logger)
self.auto3 = Auto3(self, self.logger)
self.auto_chooser = sendablechooser.SendableChooser()
self.auto_chooser.setDefaultOption('Auto1', self.auto1)
self.auto_chooser.addOption('Auto2', self.auto2)
self.auto_chooser.addOption('Auto3', self.auto3)
SmartDashboard.putData("AutoChooser", self.auto_chooser)
if self.isReal():
self.compressor = Compressor(robotmap.PCM2_CANID)
self.compressor.setClosedLoopControl(True)
self.logger.info("Compressor enabled: " +
str(self.compressor.enabled()))
else:
self.compressor = Compressor(0)
self.pilot_stick = XboxController(0)
self.copilot_stick = XboxController(1)
self.drive = DeepSpaceDrive(self.logger)
self.drive.init()
self.lift = DeepSpaceLift(self.logger)
self.lift.init()
self.claw = DeepSpaceClaw(self.logger)
self.claw.init()
self.harpoon = DeepSpaceHarpoon(self.logger)
self.harpoon.init()
def init(self):
self.logger.info("DeepSpaceHarpoon::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.ir_harpoon = AnalogInput(robotmap.IR_HARPOON)
self.state_timer = wpilib.Timer()
self.current_state = HarpoonState.HARPOON_STOWED
self.harpoon_outside_extend = wpilib.Solenoid(
robotmap.PCM2_CANID, robotmap.HARPOON_OUTSIDE_EXTEND_SOLENOID)
self.harpoon_outside_retract = wpilib.Solenoid(
robotmap.PCM2_CANID, robotmap.HARPOON_OUTSIDE_RETRACT_SOLENOID)
self.harpoon_center_extend = wpilib.Solenoid(
robotmap.PCM2_CANID, robotmap.HARPOON_CENTER_EXTEND_SOLENOID)
self.harpoon_center_retract = wpilib.Solenoid(
robotmap.PCM2_CANID, robotmap.HARPOON_CENTER_RETRACT_SOLENOID)
self.harpoon_center_extend.set(False)
self.harpoon_center_retract.set(True)
self.harpoon_outside_extend.set(False)
self.harpoon_outside_retract.set(True)
self.test_harpoon_outside = sendablechooser.SendableChooser()
self.test_harpoon_outside.setDefaultOption("Retract", 1)
self.test_harpoon_outside.addOption("Extend", 2)
self.test_harpoon_center = sendablechooser.SendableChooser()
self.test_harpoon_center.setDefaultOption("Retract", 1)
self.test_harpoon_center.addOption("Extend", 2)
SmartDashboard.putData("TestHarpoonOutside", self.test_harpoon_outside)
SmartDashboard.putData("TestHarpoonCenter", self.test_harpoon_center)
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
if not self.robot.isSimulation():
with open("/home/lvuser/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
else:
with open("/home/ubuntu/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
self.target_chooser = sendablechooser.SendableChooser()
self.target_chooser.setDefaultOption(TargetHeight.LOW.name,
TargetHeight.LOW)
self.target_chooser.addOption(TargetHeight.MIDDLE.name,
TargetHeight.MIDDLE)
self.target_chooser.addOption(TargetHeight.HIGH.name,
TargetHeight.HIGH)
self.left_right_chooser = sendablechooser.SendableChooser()
self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name,
RightLeft.RIGHT)
self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT)
self.hab_level_chooser = sendablechooser.SendableChooser()
self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name,
HabLevel.LEVEL1)
self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2)
SmartDashboard.putData("TargetChooser", self.target_chooser)
SmartDashboard.putData("LeftRightChooser", self.left_right_chooser)
SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser)
self.chosen_target = TargetHeight.LOW
self.left_right = RightLeft.RIGHT
self.hab_level = HabLevel.LEVEL1
def __init__(self, robot):
self.joy = wpilib.Joystick(0)
# Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", SetElevatorSetpoint(robot, 0));
SmartDashboard.putData("Elevator Platform", SetElevatorSetpoint(robot, 0.2));
SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3));
SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0));
SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45));
SmartDashboard.putData("Open Claw", OpenClaw(robot));
SmartDashboard.putData("Close Claw", CloseClaw(robot));
SmartDashboard.putData("Deliver Soda", Autonomous(robot));
# Create some buttons
d_up = JoystickButton(self.joy, 5)
d_right = JoystickButton(self.joy, 6)
d_down = JoystickButton(self.joy, 7)
d_left = JoystickButton(self.joy, 8)
l2 = JoystickButton(self.joy, 9)
r2 = JoystickButton(self.joy, 10)
l1 = JoystickButton(self.joy, 11)
r1 = JoystickButton(self.joy, 12)
# Connect the buttons to commands
d_up.whenPressed(SetElevatorSetpoint(robot, 0.2))
d_down.whenPressed(SetElevatorSetpoint(robot, -0.2))
d_right.whenPressed(CloseClaw(robot))
d_left.whenPressed(OpenClaw(robot))
r1.whenPressed(PrepareToPickup(robot))
r2.whenPressed(Pickup(robot))
l1.whenPressed(Place(robot))
l2.whenPressed(Autonomous(robot))
def initSmartDashboard(self):
SmartDashboard.putData("AutoFieldPosition", self.startingFieldPosition)
SmartDashboard.putData("AutoBarrierType", self.barrierType)
SmartDashboard.putData("AutoStrategy", self.strategy)
def __init__(self, robot):
self.joy = wpilib.Joystick(0)
# Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom",
SetElevatorSetpoint(robot, 0))
SmartDashboard.putData("Elevator Platform",
SetElevatorSetpoint(robot, 0.2))
SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3))
SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0))
SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45))
SmartDashboard.putData("Open Claw", OpenClaw(robot))
SmartDashboard.putData("Close Claw", CloseClaw(robot))
SmartDashboard.putData("Deliver Soda", Autonomous(robot))
# Create some buttons
d_up = JoystickButton(self.joy, 5)
d_right = JoystickButton(self.joy, 6)
d_down = JoystickButton(self.joy, 7)
d_left = JoystickButton(self.joy, 8)
l2 = JoystickButton(self.joy, 9)
r2 = JoystickButton(self.joy, 10)
l1 = JoystickButton(self.joy, 11)
r1 = JoystickButton(self.joy, 12)
# Connect the buttons to commands
d_up.whenPressed(SetElevatorSetpoint(robot, 0.2))
d_down.whenPressed(SetElevatorSetpoint(robot, -0.2))
d_right.whenPressed(CloseClaw(robot))
d_left.whenPressed(OpenClaw(robot))
r1.whenPressed(PrepareToPickup(robot))
r2.whenPressed(Pickup(robot))
l1.whenPressed(Place(robot))
l2.whenPressed(Autonomous(robot))
sd.putNumber("aNumber", 0)
auto = SendableChooser()
auto.setDefaultOption('Do Nothing', 1)
auto.addOption('Move Forward', 2)
auto.addOption('Shoot Three', 3)
# SendableRegistry.add(auto, 'Auto Mode')
blap = SendableChooser()
blap.setDefaultOption('it bad', 1)
blap.addOption('it good', 2)
blap.addOption('it something', 3)
# SendableRegistry.add(blap, 'norp')
SmartDashboard.putData(auto)
SmartDashboard.putData(blap)
i = 0
b = True
while True:
sd.putNumber("robotTime", i)
sd.putString("robotTimeString", str(i) + " seconds")
sd.putString("myTable/robotTimeString", str(i) + " seconds again")
sd.putBoolean("lightOn", b)
time.sleep(1)
i += 1
b = not b
if i == 10:
def dashboardInit(self):
SmartDashboard.putData("Turn Angle", TurnAngle())
SmartDashboard.putData("Drive Combined", DriveStraight())
def setupDashboardCommands(self):
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption(
"Do Nothing", wpilib.command.WaitCommand(3.0, "Do Nothing"))
self.autonChooser.addOption("Drive Forward", DriveTickTimed(1.0, 1.0))
self.autonChooser.addOption("Drive Backward",
DriveTickTimed(-1.0, -1.0))
self.autonChooser.addOption("Rotate Right", DriveTickTimed(1.0, -1.0))
self.autonChooser.addOption("Rotate Left", DriveTickTimed(-1.0, 1.0))
SmartDashboard.putData("Auto mode", self.autonChooser)
# Drive controls
DriveHuman.setupDashboardControls()
SmartDashboard.putData("Brake Control",
DriveHuman.createBrakeModeToggleCommand())
SmartDashboard.putData("Flip Front",
DriveHuman.createFlipFrontCommand())
# Set up utility controls
SmartDashboard.putData("Measure", Measure())
# Climber settings
SmartDashboard.putData("Full Auto Climb", ClimbUp())
if self.debug:
SmartDashboard.putData("Extend Both Legs", ExtendBothLegs())
SmartDashboard.putData("Drive to Front Sensor",
DriveToFrontSensor())
SmartDashboard.putData("Retract Front Legs", RetractFrontLegs())
SmartDashboard.putData("Drive to Back Sensor", DriveToBackSensor())
SmartDashboard.putData("Retract Back Legs", RetractBackLegs())
# Debug tools (if enabled)
if self.debug:
SmartDashboard.putData("CPU Load Test", LoadTest())
SmartDashboard.putData("Drive Subsystem", subsystems.drive)
dd = subsystems.drive.getDifferentialDrive()
if dd != None:
SmartDashboard.putData("DifferentialDrive", dd)
def showCommand(cmd):
"""Display the given command on the dashboard."""
name = cmd.getName()
name.replace("/", "_")
SmartDashboard.putData("Commands/%s" % name, cmd)
def installControl():
""" Call this method to add button to control whether rate is updated or not. """
SmartDashboard.putData("Debug Rate", DebugRate())
def robotInit(self):
SmartDashboard.putData("Performance Test", pythonTest.PythonTest())
def dashboardInit(self):
SmartDashboard.putData("test command", LimeLightAutoAlign(self.robot))
SmartDashboard.putData("Turn Angle", TurnAngle())
SmartDashboard.putData("Drive Combined", DriveStraight())
