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