class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.C = Component() # Components inits all connected motors, sensors, and joysticks. See components.py.
# Setup subsystems
self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
self.belt = Belt(self.C.beltM)
self.beltAxis = BeltAxis(self.C.beltAxisM)
self.shoot = Shooter(self.C.flipM, self.C.shootM)
self.lift = Lift(self.C.lift1M, self.C.lift2M)
self.sonic = Sonic(self.C.sonic)
self.arduino = Arduino(self.C.arduino)
self.guide = Guiding(self.sonic, self.drive)
self.sd = SmartDashboard
self.sd.putBoolean("autoMove", False)
self.autoLoop = 0
def operatorControl(self):
self.C.driveTrain.setSafetyEnabled(True) # keeps robot from going crazy if connection with DS is lost
while self.isOperatorControl() and self.isEnabled():
distance = self.sonic.getAverage()
print(distance)
self.sd.putDouble('distance', distance)
if (5.25 < distance and distance < 5.75):
self.sd.putBoolean("Ready", True)
else:
self.sd.putBoolean("Ready", False)
if (self.C.middleJ.getRawButton(1) == True):
self.guide.guideSonic()
# Drive
self.drive.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
# Components
self.belt.run(self.C.rightJ.getRawButton(4), self.C.rightJ.getRawButton(5))
self.beltAxis.run(self.C.rightJ.getRawButton(3), self.C.rightJ.getY(), self.C.beltAxisTS.get(), self.C.beltAxisBS.get())
self.lift.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
self.shoot.run(self.C.rightJ.getRawButton(2), self.C.beltAxisTS.get(), self.C.rightJ.getRawButton(1), self.C.flipS.get()) # weird argument in the middle makes sure shooter doesn't turn on when belt it up
wpilib.Timer.delay(0.005) # wait for a motor update time
def autonomous(self):
"""This function is called periodically during autonomous."""
if (self.sd.getBoolean("autoMove") == True):
distance = self.sonic.getAverage()
while (self.autoLoop < 2000 and distance > 5):
self.drive.set(-1, 0)
wpilib.Timer.delay(0.005)
self.autoLoop = self.autoLoop + 1
def test(self):
"""This function is called periodically during test mode."""
class Randy(wpilib.TimedRobot):
def robotInit(self):
self.C = Component(
) # Components inits all connected motors, sensors, and joysticks. See inits.py.
# Setup subsystems
self.driverStation = wpilib.DriverStation.getInstance()
self.drive = Chassis(self.C.driveTrain, self.C.gyroS,
self.C.driveYEncoderS)
self.lights = Lights()
self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS,
self.C.jawsLimitS, self.C.metaboxLimitS,
self.C.jawsM, self.C.elevatorM, self.C.intakeM,
self.C.jawsSol)
self.winch = Winch(self.C.winchM)
self.power = Power()
# Joysticks
self.joystick = wpilib.XboxController(0)
self.leftJ = wpilib.Joystick(1)
# default to rainbow effect
self.lights.run({'effect': 'rainbow'})
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('station', 2)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
'''Components'''
# Rumble
averageDriveCurrent = self.power.getAverageCurrent([0, 1, 14, 15])
if (averageDriveCurrent > 8):
self.joystick.setRumble(0, 1)
else:
self.joystick.setRumble(0, 0)
print(self.metabox.getEncoder())
'''
TODO: calibrate sparks
'''
# Drive
self.drive.run(self.joystick.getRawAxis(0),
self.joystick.getRawAxis(1),
self.joystick.getRawAxis(4))
# MetaBox
self.metabox.run(
self.leftJ.getY(), # elevator rate of change
self.leftJ.getRawButton(1), # run intake wheels in
self.leftJ.getRawButton(3), # open jaws
self.leftJ.getRawButton(2), # run intake wheels out
self.leftJ.getRawButton(4), # go to bottom
self.leftJ.getRawAxis(2), # set angle of jaws
self.leftJ.getRawButton(8)) # calibrate elevator
# Lights
self.lights.setColor(self.driverStation.getAlliance())
if (self.driverStation.getMatchTime() < 30
and self.driverStation.getMatchTime() != -1):
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 200})
elif (helpers.deadband(self.leftJ.getY(), 0.1) != 0):
self.lights.run({'effect': 'stagger'})
elif (self.leftJ.getRawButton(1) or self.leftJ.getRawButton(2)):
self.lights.run({'effect': 'flash', 'fade': False, 'speed': 20})
else:
self.lights.run({'effect': 'rainbow'})
def teleopInit(self):
"""This function is run once each time the robot enters teleop mode."""
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
# Init autonomous
self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS,
self.C.gyroS, self.metabox,
self.driverStation)
# reset state
self.autonomousRoutine.state = 0
def autonomousPeriodic(self):
self.autonomousRoutine.run() # see autonomous.py
def test(self):
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
