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