def execute(self):
oi = OI.get()
elevator = self.elevator
power = oi.get_elevator_manual_command()
if abs(power) < 0.05:
power = 0
elevator.set_power(power)
if self.elevator.is_at_bottom():
self.elevator.talon_master.setQuadraturePosition(0, 0)
print(elevator.get_elevator_position())
def execute(self):
oi = OI.get()
intake = self.intake
pwr = oi.get_outtake_command()
if abs(pwr) < 0.05:
pwr = 0
max_intake_power = 0.5
if pwr > max_intake_power:
pwr = max_intake_power
intake.run_intake(pwr)
if intake.has_acquired_cube() and pwr > 0:
oi.rumble_op()
else:
oi.unrumble_op()
def execute(self):
oi = OI.get()
intake = self.intake
if oi.arm_is_down():
intake.set_arm_state(ArmState.DOWN)
else:
intake.set_arm_state(ArmState.UP)
if self.last_arm_state != ArmState.UP:
self.arm_timer.start()
self.arm_timer.reset()
self.arm_timer_latch = True
self.last_arm_state = self.intake.get_arm_state()
if self.arm_timer.get() > 0.5:
self.arm_timer_latch = False
self.arm_timer.stop()
if self.arm_timer_latch:
intake.run_intake(0.1)
else:
self.arm_timer.stop()
if oi.intake_is_active():
# intake.set_grab_state(GrabState.OUT)
pwr = oi.get_outtake_command()
if abs(pwr) < 0.05:
pwr = 0
max_intake_power = 0.5
if pwr > max_intake_power:
pwr = max_intake_power
intake.run_intake(pwr)
if intake.has_acquired_cube() and pwr > 0:
oi.rumble_op()
else:
oi.unrumble_op()
else:
intake.run_intake(0)
oi.unrumble_op()
if oi.arm_is_open():
intake.set_grab_state(GrabState.OUT)
else:
intake.set_grab_state(GrabState.IN)
def execute(self):
oi = OI.get()
drive = self.drivetrain
elevator = self.elevator
if oi.get_fine_forward():
power = .1
elif oi.get_fine_backward():
power = -.1
else:
power = self.get_speed_scale(elevator.get_elevator_position()) * \
mathutils.signed_power(oi.get_left_power(),2)
if oi.get_fine_left_turn():
turn = -.1
elif oi.get_fine_right_turn():
turn = .1
else :
turn = mathutils.signed_power(oi.get_turn_command(), 2)
if oi.get_spot_turn():
drive.tank_drive(.1,-.1)
else:
drive.arcade_drive(power, turn * 0.65)
def robotInit(self):
# Start up continuous processes
# In simulation, cd is code dir. On the robot, it's something else so we need to use abs dir
basedir = get_basedir()
dashboard2.run(basedir)
DashboardUpdateCommand().start()
OIUpdateCommand().start()
# CheckFaults([self.elevator, self.forklift]).start()
wpilib.CameraServer.launch('vision.py:main')
dashboard2.add_graph("Speed", lambda: 0)
dashboard2.add_graph("CTE", lambda: 0)
dashboard2.add_graph("Lookahead", lambda: 0)
dashboard2.add_graph("Goal Distance", lambda: 0)
# Actions
elev_manual_command = OpElevatorManualCommand(self.elevator)
elev_move_to_top = MoveElevatorCommand(self.elevator, 62)
elev_move_to_bottom = MoveElevatorCommand(self.elevator, 0)
elev_zero = ElevatorZeroCommand(self.elevator)
elev_intake_switch = CommandGroup()
elev_intake_switch.addSequential(MoveElevatorCommand(
self.elevator, 30))
elev_intake_switch.addSequential(
MoveIntakeCommand(self.intake, ArmState.DOWN))
intake_open = OpenIntakeCommand(self.intake)
intake_toggle = ToggleIntakeCommand(self.intake)
intake_run = RunIntakeCommand(self.intake)
drop_forks = DropForkliftCommand(self.forklift)
climb_cmd = Climb(climber=self.forklift, elevator=self.elevator)
OI.get().exec_while_condition(
condition=OI.get().elevator_is_manual_control,
cmd=elev_manual_command)
OI.get().add_action_listener(
condition=OI.get().elevator_move_to_bottom,
action=elev_move_to_bottom.start)
OI.get().add_action_listener(condition=OI.get().elevator_move_to_top,
action=elev_move_to_top.start)
OI.get().add_action_listener(
condition=OI.get().elevator_move_to_switch,
action=elev_intake_switch.start)
OI.get().add_action_listener(condition=OI.get().elevator_zero,
action=elev_zero.start)
OI.get().add_action_listener(condition=OI.get().do_drop_forks,
action=drop_forks.start)
OI.get().exec_while_condition(condition=OI.get().do_climb,
cmd=climb_cmd)
OI.get().add_action_listener(condition=OI.get().toggle_arm,
action=intake_toggle.start)
OI.get().exec_while_condition(condition=OI.get().arm_is_open,
cmd=intake_open)
OI.get().exec_while_condition(condition=OI.get().intake_is_active,
cmd=intake_run)
self.side_chooser = dashboard2.add_chooser("Starting Position")
self.side_chooser.add_option("Left", game_data.Side.LEFT)
self.side_chooser.add_option("Center", game_data.Side.CENTER)
self.side_chooser.add_option("Right", game_data.Side.RIGHT)
self.side_chooser.set_default("Center")
# Auto modes
auto_switch_only = SwitchOnlyMonolith(drive=self.drivetrain,
elevator=self.elevator,
intake=self.intake)
auto_scale_only = ScaleOnly(drive=self.drivetrain,
elevator=self.elevator,
intake=self.intake)
auto_scale_double = DoubleScale(drive=self.drivetrain,
elevator=self.elevator,
intake=self.intake)
auto_switch_scale = ScaleSwitchSameSide(drive=self.drivetrain,
elevator=self.elevator,
intake=self.intake)
# auto_vault = VaultOnly(drive=self.drivetrain, intake=self.intake)
auto_drive_simple = TimeDriveCommand(drive=self.drivetrain,
power=0.3,
time=3)
self.auto_chooser = dashboard2.add_chooser("Autonomous")
self.auto_chooser.add_option("Switch Only", auto_switch_only)
self.auto_chooser.add_option("Scale Only", auto_scale_only)
self.auto_chooser.add_option("2x Scale", auto_scale_double)
self.auto_chooser.add_option("Scale -> Switch", auto_switch_scale)
# self.auto_chooser.add_option("Switch and Scale", auto_switch_scale)
# self.auto_chooser.add_option("Vault Only", auto_vault)
self.auto_chooser.add_option(
"Drive Forward",
PursuitDriveCommand(acc=4,
cruise_speed=8,
waypoints=[Pose(1.5, 0, 0),
Pose(10, 0, 0)],
drive=self.drivetrain))
self.auto_chooser.set_default("Drive Forward")
def end(self):
OI.get().unrumble_op()
self.intake.run_intake(0)
def end(self):
OI.get().unrumble_op()