class Pneumatics(Subsystem):
def __init__(self, robot):
super().__init__()
self.counter = 0
self.double_solenoid = DoubleSolenoid(0, 1)
self.compressor = Compressor(0)
self.compressor.setClosedLoopControl(True)
def actuate_solenoid(self, direction=None):
if direction == 'open':
self.double_solenoid.set(DoubleSolenoid.Value.kForward)
elif direction == 'close':
self.double_solenoid.set(DoubleSolenoid.Value.kReverse)
else:
pass
def log(self):
pass
class MyRobot(wpilib.TimedRobot):
robot_mode = RobotMode.AUTO
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 autonomousInit(self):
self.compressor.setClosedLoopControl(True)
self.logger.info("autonomousInit Compressor enabled: " +
str(self.compressor.enabled()))
self.robot_mode = RobotMode.AUTO
self.logger.info("MODE: autonomousInit")
self.auto_selected = self.auto_chooser.getSelected()
self.pilot_stick.config()
self.copilot_stick.config()
self.drive.config(self.isSimulation())
self.lift.config(self.isSimulation())
self.claw.config(self.isSimulation())
self.harpoon.config(self.isSimulation())
self.auto_selected.init()
def autonomousPeriodic(self):
self.robot_mode = RobotMode.AUTO
if self.timer.hasPeriodPassed(1.0):
self.logger.info("MODE: autonomousPeriodic")
self.auto_selected.iterate()
self.drive.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.lift.iterate(self.robot_mode, self.isSimulation(),
self.pilot_stick, self.copilot_stick)
self.claw.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.harpoon.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
def teleopInit(self):
self.compressor.setClosedLoopControl(True)
self.logger.info("teleopInit Compressor enabled: " +
str(self.compressor.enabled()))
self.robot_mode = RobotMode.TELE
self.logger.info("MODE: teleopInit")
self.pilot_stick.config()
self.copilot_stick.config()
self.drive.config(self.isSimulation())
self.lift.config(self.isSimulation())
self.claw.config(self.isSimulation())
self.harpoon.config(self.isSimulation())
def teleopPeriodic(self):
self.robot_mode = RobotMode.TELE
if self.timer.hasPeriodPassed(1.0):
self.logger.info("MODE: teleopPeriodic")
self.drive.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.lift.iterate(self.robot_mode, self.isSimulation(),
self.pilot_stick, self.copilot_stick)
self.claw.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.harpoon.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
if self.lift.current_lift_preset != LiftPreset.LIFT_PRESET_STOW and self.lift.on_target:
self.pilot_stick.pulseRumble(1.0)
self.copilot_stick.pulseRumble(1.0)
else:
self.pilot_stick.stopRumble()
self.copilot_stick.stopRumble()
def disabledInit(self):
self.robot_mode = RobotMode.DISABLED
self.logger.info("MODE: disabledInit")
self.drive.disable()
self.lift.disable()
self.claw.disable()
self.harpoon.disable()
def disabledPeriodic(self):
self.robot_mode = RobotMode.DISABLED
if self.timer.hasPeriodPassed(1.0):
self.logger.info("MODE: disabledPeriodic")
def testInit(self):
self.robot_mode = RobotMode.TEST
self.logger.info("MODE: testInit")
self.pilot_stick.config()
self.copilot_stick.config()
self.drive.config(self.isSimulation())
self.lift.config(self.isSimulation())
self.claw.config(self.isSimulation())
self.harpoon.config(self.isSimulation())
def testPeriodic(self):
self.robot_mode = RobotMode.TEST
if self.timer.hasPeriodPassed(1.0):
self.logger.info("MODE: testPeriodic")
self.drive.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.lift.iterate(self.robot_mode, self.isSimulation(),
self.pilot_stick, self.copilot_stick)
self.claw.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
self.harpoon.iterate(self.robot_mode, self.pilot_stick,
self.copilot_stick)
class Robot(IterativeRobot):
def robotInit(self):
self.navx = Navx.Navx()
self.climber = Climber.Climber()
self.limelight = Limelight.Limelight()
self.drive = Drive.Drive(self)
self.intake = None
self.lift = Lift.Lift(self)
self.intake = Intake.Intake(self)
self.lift._intake = self.intake
self.autonomous = AutonomousBaseSpline.AutonomousBaseSpline(self)
self.controls = DualRemote(self)
self.compressor = Compressor(COMPRESSOR_PIN)
self.driverStation = DriverStation.getInstance()
self.a0 = AnalogInput(0)
self.a1 = AnalogInput(1)
self.a2 = AnalogInput(2)
self.a3 = AnalogInput(3)
self.runAutoOne = True
#self.liftRTS = RTS("liftRTS",8)
#self.liftRTS.addTask(self.lift.periodic)
#self.liftRTS.start()
def autonomousInit(self):
self.runAutoOne = True
def autonomousPeriodic(self):
if self.runAutoOne:
self.lift.zeroEncoder()
self.lift.setLoc(0)
self.drive.resetDistance()
self.navx.resetAngle()
selectedAuto = self.autoSelector()
gameData = self.driverstation.getGameSpecificMessage()
self.autonomous.autonomousInit(gameData, selectedAuto)
if self.autonomous.is_alive():
self.autonomous.terminate()
self.autonomous.start()
self.runAutoOne = False
def teleopInit(self):
if self.autonomous.is_alive():
self.autonomous.terminate()
self.drive.setSpeedLimit(1)
self.drive.tankDrive(0, 0)
self.compressor.setClosedLoopControl(True)
self.drive.resetDistance()
def teleopPeriodic(self):
a = 0
while self.isOperatorControl() and self.isEnabled():
self.controls.periodic()
self.limelight.mutiPipeline()
self.intake.periodic()
self.lift.periodic()
b = self.lift.getEncoderVal()
if a < b:
a = b
SmartDashboard.putNumber("pipeline id",
self.limelight.getPipeline())
SmartDashboard.putBoolean("inake hasCube", self.intake.hasCube())
#drive.periodic()
#SmartDashboard.putNumber("liftRTS hz", self.liftRTS.getHz())
SmartDashboard.putNumber("0", self.a0.getAverageVoltage())
SmartDashboard.putNumber("1", self.a1.getAverageVoltage())
SmartDashboard.putNumber("2", self.a2.getAverageVoltage())
SmartDashboard.putNumber("3", self.a3.getAverageVoltage())
SmartDashboard.putNumber("Lift", a)
SmartDashboard.putNumber("Drive Right Dist",
self.drive.getRightDistance())
SmartDashboard.putNumber("Drive Left Dist",
self.drive.getLeftDistance())
SmartDashboard.putNumber("pitch", self.navx.getPitch())
def autoSelector(self):
voltage = -1
number = "Baseline"
if self.a0.getAverageVoltage() > voltage:
number = "Left"
voltage = self.a0.getAverageVoltage()
if self.a1.getAverageVoltage() > voltage:
number = "Middle"
voltage = self.a1.getAverageVoltage()
if self.a2.getAverageVoltage() > voltage:
number = "None"
voltage = self.a2.getAverageVoltage()
if self.a3.getAverageVoltage() > voltage:
number = "Right"
voltage = self.a3.getAverageVoltage()
print("volt:", voltage)
return number
class Robot(TimedRobot):
"""
Command-based robots inherit from the TimedRobot class.
"""
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
self.compressor = Compressor()
self.compressor.setClosedLoopControl(True)
robotsubsystems.init()
oi.init()
def robotPeriodic(self):
"""
This function is called every robot packet, no matter the mode. Use
this for items like diagnostics that you want ran during disabled,
autonomous, teleoperated and test.
This runs after the mode specific periodic functions, but before
LiveWindow and SmartDashboard integrated updating.
"""
pass
def disabledInit(self):
"""
This function is called once each time the robot enters Disabled mode.
You can use it to reset any subsystem information you want to clear when
the robot is disabled.
"""
pass
def disabledPeriodic(self):
"""
This function is repeatedly while the robot is in Disabled mode.
You generally will not need to alter this method.
"""
Scheduler.getInstance().run()
def autonomousInit(self):
"""
This method is called once at the start of autonomous.
Put anything in this method that needs to happen when autonomous starts.
"""
pass
def autonomousPeriodic(self):
"""
This function is called periodically during autonomous.
You generally will not need to alter this method.
"""
Scheduler.getInstance().run()
def teleopInit(self):
"""
This method is called once at the start of teleop.
Put anything in this method that needs to happen when teleop starts.
"""
pass
def teleopPeriodic(self):
"""
This function is called periodically during teleop.
You generally will not need to alter this method.
"""
Scheduler.getInstance().run()
def testPeriodic(self):
"""
This method is not called during competition.
You can put test code in here that is tested from driver station.
"""
self.pdp = PowerDistributionPanel(0)
self.pdp.clearStickyFaults()
self.compressor.clearAllPCMStickyFaults()
