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):
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, 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):
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):
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 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()