def __init__(self):
self.mainCompressor = Compressor(PCMID)
self.intakeSolenoid = Solenoid(PCMID, intakeSolenoidChannel)
self.isExtended = False
self.mainCompressor.start()
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()
class Pneumatics(Component):
def __init__(self):
super().__init__()
self.comp = Compressor()
def update(self):
"""
Monitors the PDP for amp draw, and disables the compressor if amp draw is above a threshold to prevent brownouts.
:return:
"""
self.comp.start()
def stop(self):
"""Disables EVERYTHING. Only use in case of critical failure"""
#self.comp.stop()
pass
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 Solenoid:
compressor = Compressor()
PCM_ID = 20
def __init__(self, forward_port, reverse_port):
'''
Creates a Solenoid object.
Takes two arguments, forward_port, and reverse_port.
These are the port numbers on the PCM that each solenoid
in the doublesolenoid is wired to.
The doublesolenoid is the solenoid that can invert pushing and pulling
on an actuator.
'''
self.wpilib_solenoid = DoubleSolenoid(self.PCM_ID, forward_port,
reverse_port)
self.forward_port = forward_port
self.reverse_port = reverse_port
self.inverted = False
self.set_forward()
def set_forward(self):
self.set_wrapped_solenoid(Reverse if self.is_inverted() else Forward)
def set_reverse(self):
self.set_wrapped_solenoid(Forward if self.is_inverted() else Reverse)
def extend(self):
self.set_forward()
def retract(self):
self.set_reverse()
def set_invert(self, invert):
'''
If `invert` is true, this inverts the Solenoids
where forwards is reverse and vice versa.
If `invert` is false, it brings it back to its normal state.
'''
self.inverted = invert
def is_inverted(self):
return self.inverted
def set_wrapped_solenoid(self, value):
self.wpilib_solenoid.set(value)
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
class PneumaticsSubsystem(Subsystem):
def __init__(self):
self.mainCompressor = Compressor(PCMID)
self.intakeSolenoid = Solenoid(PCMID, intakeSolenoidChannel)
self.isExtended = False
self.mainCompressor.start()
def initDefaultCommand(self):
pass
def flipSolenoid(self):
if self.isExtended == False:
self.isExtended = True
else:
self.isExtended == False
self.intakeSolenoid.set(isExtended)
def getIsExtended(self):
return self.isExtended
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, robot):
super().__init__()
self.robot = robot
self.armMotor = CANTalon(4)
self.wheelMotor = CANTalon(5)
self.frontSwitch = DigitalInput(8)
self.backSwitch = DigitalInput(9)
self.comp = Compressor()
self.comp.enabled()
self.armMotor.enableBrakeMode(True)
self.wheelMotor.enableBrakeMode(True)
self.potentiometer = AnalogPotentiometer(3, 270, -193)
#self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02)
self.position = 0
def __init__(self):
self.timer = Timer()
self.driver_station = DriverStation(controller=XboxController(0))
# self.gyroscope = AHRS.create_i2c()
# self.left_encoder = Encoder()
# self.right_encoder = Encoder()
self.drivetrain = TankDrivetrain(timer=self.timer, left_motors=[TalonSRX(10), TalonSRX(6)],
right_motors=[TalonSRX(12), TalonSRX(18)])
self.pneumatics = Pneumatics(compressor=Compressor(0), start_compressor=True, timer=self.timer)
self.beak = BeakMechanism(beak_solenoid=DoubleSolenoid(0, 4, 5), diag_solenoid=DoubleSolenoid(0, 0, 1),
driver_station=self.driver_station, timer=self.timer, cooldown=0.5)
# self.odometry = EncoderOdometry(left_encoder=self.left_encoder, right_encoder=self.right_encoder, gyro=self.gyroscope)
'''
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 arm(Component):
#set up variables
def __init__(self, robot):
super().__init__()
self.robot = robot
self.armMotor = CANTalon(4)
self.wheelMotor = CANTalon(5)
self.frontSwitch = DigitalInput(8)
self.backSwitch = DigitalInput(9)
self.comp = Compressor()
self.comp.enabled()
self.armMotor.enableBrakeMode(True)
self.wheelMotor.enableBrakeMode(True)
self.potentiometer = AnalogPotentiometer(3, 270, -193)
#self.pidArm = PIDController(0.0, 0.0, 0.0, 0.0, self.potentiometer, self.armMotor, 0.02)
self.position = 0
def armAuto(self, upValue, downValue, target, rate=0.3):
'''
if self.getPOT() <= target:
self.armMotor.set(0)
'''
if upValue == 1:
self.armMotor.set(rate * -1)
elif downValue == 1:
self.armMotor.set(rate)
else:
self.armMotor.set(0)
def armUpDown(self, left, right, controllerA, rate=0.3):
rate2 = rate*1.75
if(self.backSwitch.get() == False or self.frontSwitch.get() == False): #Checking limit switches
self.armMotor.set(0)
if(left >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(rate2)
# else:
self.armMotor.set(rate)
elif(right >= 0.75):#if tripped, disallow further movement
# if(controllerA == True):
# self.armMotor.set(-rate2)
# else:
self.armMotor.set(rate * -1)
elif(left < 0.75 and right < 0.75):
self.armMotor.set(0)
def wheelSpin(self, speed):
currentSpeed = 0
if (speed > 0.75):
currentSpeed = -1
elif(speed < -0.75):
currentSpeed = 1
self.wheelMotor.set(currentSpeed)
def getPOT(self):
return (self.potentiometer.get()*-1)
def getBackSwitch(self):
return self.backSwitch.get()
def getFrontSwitch(self):
return self.frontSwitch.get()
from constants import PCM_ID
from wpilib import DoubleSolenoid as WPI_DoubleSolenoid, Compressor
Compressor(PCM_ID)
class DoubleSolenoid(WPI_DoubleSolenoid):
def __init__(self, forward_channel, reverse_channel):
super().__init__(PCM_ID, forward_channel, reverse_channel)
def forward(self):
self.set(self.Value.kForward)
def reverse(self):
self.set(self.Value.kReverse)
def __init__(self, robot):
super().__init__()
self.counter = 0
self.double_solenoid = DoubleSolenoid(0, 1)
self.compressor = Compressor(0)
self.compressor.setClosedLoopControl(True)
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
from grt.sensors.attack_joystick import Attack3Joystick
from grt.sensors.xbox_joystick import XboxJoystick
from grt.core import SensorPoller
from grt.mechanism.drivetrain import DriveTrain
from grt.mechanism.drivecontroller import ArcadeDriveController
from grt.sensors.encoder import Encoder
from grt.mechanism.mechcontroller import MechController
from grt.sensors.navx import NavX
from grt.macro.straight_macro import StraightMacro
from grt.mechanism.pickup import Pickup
from grt.mechanism.manual_shooter import ManualShooter
from queue import Queue
#Compressor initialization
c = Compressor()
c.start()
#Manual pickup Talons and Objects
pickup_achange_motor1 = CANTalon(45)
pickup_achange_motor2 = CANTalon(46)
pickup_achange_motor1.changeControlMode(CANTalon.ControlMode.Follower)
pickup_achange_motor1.set(47)
pickup_achange_motor1.reverseOutput(True)
pickup_roller_motor = CANTalon(8)
pickup = Pickup(pickup_achange_motor1, pickup_achange_motor2,
pickup_roller_motor)
def __init__(self):
super().__init__()
self.comp = Compressor()
quickdebug.add_printables(self, [('comp enabled', self.comp.enabled)])
def __init__(self):
super().__init__()
self.comp = Compressor()
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()