def robotInit(self):
self.controller = XboxController(0)
self.controller2 = XboxController(1)
#self.lmotor = wpilib.CANTalon(1)
#self.rmotor = wpilib.CANTalon(0)
self.drive = driveTrain(self)
self.robotArm = arm(self)
self.climber = lift(self)
self.pixy = Pixy()
self.drive.reset()
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
# Initialize Components functions
self.components = {
'drive' : self.drive,
'arm' : self.robotArm,
'lift' : self.climber,
'pixy' : self.pixy
}
# Initialize Smart Dashboard
self.dash = SmartDashboard()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
self.potentiometer = ('Arm Potentiometer', 0)
self.dash.putNumber('ControlType', 0)
self.dash.putBoolean('Front Switch', 0)
self.dash.putBoolean('Back Switch', 0)
self.drive.log()
def main():
"""
Main function for running robot code, should never exit.
Currently this ONLY works with an xbox 360 controller.
"""
controller = XboxController()
while True:
arcade_drive(controller, debug=True)
shooter(controller, debug=True)
dart(controller)
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.controller = XboxController(0)
self.lmotor = wpilib.CANTalon(0)
self.rmotor = wpilib.CANTalon(1)
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
#self.autonomous_modes = AutonomousModeSelector('autonomous')
# Initialize the smart dashboard display elements
self.sd = SmartDashboard()
self.sd.putNumber("Random", 0) # Send initialization packet
def sendRandomData(self, upper, lower, step):
self.sd.putNumber("Random", random.randrange(upper, lower, step))
def disabled(self):
while self.isDisabled():
wpilib.Timer.delay(0.01) # Wait for 0.01 seconds
def autonomous(self):
self.autonomous_modes.run()
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def operatorControl(self):
wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
self.lmotor.set(self.controller.getLeftY())
self.rmotor.set(self.controller.getRightY())
self.sendRandomData(-10,10,1)
def test(self):
wpilib.LiveWindow.run()
def robotInit(self):
self.controller = XboxController(0)
self.lmotor = wpilib.CANTalon(0)
self.rmotor = wpilib.CANTalon(1)
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
#self.autonomous_modes = AutonomousModeSelector('autonomous')
# Initialize the smart dashboard display elements
self.sd = SmartDashboard()
self.sd.putNumber("Random", 0) # Send initialization packet
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.controller = XboxController(0)
self.controller2 = XboxController(1)
#self.lmotor = wpilib.CANTalon(1)
#self.rmotor = wpilib.CANTalon(0)
self.drive = driveTrain(self)
self.robotArm = arm(self)
self.climber = lift(self)
self.pixy = Pixy()
self.drive.reset()
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
# Initialize Components functions
self.components = {
'drive' : self.drive,
'arm' : self.robotArm,
'lift' : self.climber,
'pixy' : self.pixy
}
# Initialize Smart Dashboard
self.dash = SmartDashboard()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
self.potentiometer = ('Arm Potentiometer', 0)
self.dash.putNumber('ControlType', 0)
self.dash.putBoolean('Front Switch', 0)
self.dash.putBoolean('Back Switch', 0)
self.drive.log()
def disabled(self):
self.drive.reset()
#self.drive.disablePIDs()
while self.isDisabled():
wpilib.Timer.delay(0.01) # Wait for 0.01 seconds
def autonomous(self):
self.drive.reset()
self.drive.enablePIDs()
while self.isAutonomous() and self.isEnabled():
# Run the actual autonomous mode
self.potentiometer = ('Arm Potentiometer', self.robotArm.getPOT())
self.drive.log()
self.autonomous_modes.run()
def operatorControl(self):
# Resetting encoders
self.drive.reset()
#self.drive.enablePIDs()
while self.isOperatorControl() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getLeftTrigger(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw(), self.controller2.getButtonA(), rate=0.5)
self.robotArm.wheelSpin(self.controller2.getLeftY())
self.climber.climbUpDown(self.controller2.getLeftBumper(), self.controller2.getRightBumper())
self.drive.log()
wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME)
# Send encoder data to the smart dashboard
self.dash.putNumber('Arm Potentiometer', self.robotArm.getPOT())
#self.dash.putBoolean('Back Arm Switch', self.robotArm.getFrontSwitch())
#self.dash.putBoolean('Front Arm Switch', self.robotArm.getBackSwitch())
def test(self):
wpilib.LiveWindow.run()
self.drive.reset()
self.drive.enablePIDs()
while self.isTest() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw())
'''
def robotInit(self):
self.drive = driveTrain(self)
self.interpret = interpret()
self.controllerOne = XboxController(0)
self.controllerTwo = XboxController(1)
self.speedLimiter = 1 #1 = standard speed, greater than 1 to slow down, less than 1 to speed up
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
self.dash = SmartDashboard()
self.positionChooser = wpilib.SendableChooser()
self.positionChooser.addDefault('Position Chooser', '1')
self.positionChooser.addObject('left', 'left')
self.positionChooser.addObject('right', 'right')
self.positionChooser.addObject('center', 'center')
self.switchLscaleL = wpilib.SendableChooser()
self.switchLscaleL.addDefault('Switch and Scale LEFT', '1')
self.switchLscaleL.addObject('Scale', 'scale')
self.switchLscaleL.addObject('Switch', 'switch')
#switchLscaleL.addObject('PrepScaleScore', '4')
self.switchLscaleL.addDefault(
'Drive', 'drive') ##Default for all sendable Choosers
self.switchLscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleR = wpilib.SendableChooser()
self.switchRscaleR.addDefault('Switch and Scale RIGHT', '1')
self.switchRscaleR.addObject('Scale', 'scale')
self.switchRscaleR.addObject('Switch', 'switch')
#switchRscaleR.addObject('PrepScaleScore', '4')
self.switchRscaleR.addDefault('Drive', 'drive')
self.switchRscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleL = wpilib.SendableChooser()
self.switchRscaleL.addDefault('Switch RIGHT, Scale LEFT', '1')
self.switchRscaleL.addObject('Scale', 'scale')
self.switchRscaleL.addObject('Switch', 'switch')
#switchRscaleL.addObject('PrepScaleScore', '4')
self.switchRscaleL.addDefault('Drive', 'drive')
self.switchRscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchLscaleR = wpilib.SendableChooser()
self.switchLscaleR.addDefault('Switch LEFT, Scale RIGHT', '1')
self.switchLscaleR.addObject('Scale', 'scale')
self.switchLscaleR.addObject('Switch', 'switch')
#switchLscaleR.addObject('PrepScaleScore', '4')
self.switchLscaleR.addDefault('Drive', 'drive')
self.switchLscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
#print('Dashboard Test')
wpilib.SmartDashboard.putData('Starting Position',
self.positionChooser)
wpilib.SmartDashboard.putData('Switch and Scale Left',
self.switchLscaleL)
wpilib.SmartDashboard.putData('Switch Right, Scale Left',
self.switchRscaleL)
wpilib.SmartDashboard.putData('Switch and Scale Right',
self.switchRscaleR)
wpilib.SmartDashboard.putData('Switch Left, Scale Right',
self.switchLscaleR)
#self.dash.putData('Switch Left, Scale Right', switchLscaleR)
self.dash.putString('SanityCheck', '1')
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
self.drive = driveTrain(self)
self.interpret = interpret()
self.controllerOne = XboxController(0)
self.controllerTwo = XboxController(1)
self.speedLimiter = 1 #1 = standard speed, greater than 1 to slow down, less than 1 to speed up
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
self.dash = SmartDashboard()
self.positionChooser = wpilib.SendableChooser()
self.positionChooser.addDefault('Position Chooser', '1')
self.positionChooser.addObject('left', 'left')
self.positionChooser.addObject('right', 'right')
self.positionChooser.addObject('center', 'center')
self.switchLscaleL = wpilib.SendableChooser()
self.switchLscaleL.addDefault('Switch and Scale LEFT', '1')
self.switchLscaleL.addObject('Scale', 'scale')
self.switchLscaleL.addObject('Switch', 'switch')
#switchLscaleL.addObject('PrepScaleScore', '4')
self.switchLscaleL.addDefault(
'Drive', 'drive') ##Default for all sendable Choosers
self.switchLscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleR = wpilib.SendableChooser()
self.switchRscaleR.addDefault('Switch and Scale RIGHT', '1')
self.switchRscaleR.addObject('Scale', 'scale')
self.switchRscaleR.addObject('Switch', 'switch')
#switchRscaleR.addObject('PrepScaleScore', '4')
self.switchRscaleR.addDefault('Drive', 'drive')
self.switchRscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleL = wpilib.SendableChooser()
self.switchRscaleL.addDefault('Switch RIGHT, Scale LEFT', '1')
self.switchRscaleL.addObject('Scale', 'scale')
self.switchRscaleL.addObject('Switch', 'switch')
#switchRscaleL.addObject('PrepScaleScore', '4')
self.switchRscaleL.addDefault('Drive', 'drive')
self.switchRscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchLscaleR = wpilib.SendableChooser()
self.switchLscaleR.addDefault('Switch LEFT, Scale RIGHT', '1')
self.switchLscaleR.addObject('Scale', 'scale')
self.switchLscaleR.addObject('Switch', 'switch')
#switchLscaleR.addObject('PrepScaleScore', '4')
self.switchLscaleR.addDefault('Drive', 'drive')
self.switchLscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
#print('Dashboard Test')
wpilib.SmartDashboard.putData('Starting Position',
self.positionChooser)
wpilib.SmartDashboard.putData('Switch and Scale Left',
self.switchLscaleL)
wpilib.SmartDashboard.putData('Switch Right, Scale Left',
self.switchRscaleL)
wpilib.SmartDashboard.putData('Switch and Scale Right',
self.switchRscaleR)
wpilib.SmartDashboard.putData('Switch Left, Scale Right',
self.switchLscaleR)
#self.dash.putData('Switch Left, Scale Right', switchLscaleR)
self.dash.putString('SanityCheck', '1')
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
#self.drive.encoderReset()
def interperetDashboard(self):
startingPosition = self.positionChooser.getSelected()
gameData = DriverStation.getInstance().getGameSpecificMessage()
gameData = gameData[:-1]
gameData = gameData.upper()
switchPosition = gameData[0]
scalePosition = gameData[1]
objective = 'drive'
if gameData == 'LL':
objective = self.switchLscaleL.getSelected()
elif gameData == 'LR':
objective = self.switchLscaleR.getSelected()
elif gameData == 'RL':
objective = self.switchRscaleL.getSelected()
elif gameData == 'RR':
objective = self.switchRscaleR.getSelected()
else:
objective = 'drive'
print(gameData)
print(startingPosition)
print(switchPosition)
print(scalePosition)
print(objective)
if objective == 'switch':
if (startingPosition == 'left'
and switchPosition == 'L') or (startingPosition == 'right'
and switchPosition == 'R'):
self.auton = autonNearSwitch(startingPosition, switchPosition,
scalePosition, self.drive)
elif startingPosition == 'center':
self.auton = autonCenterEitherSwitch(startingPosition,
switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and switchPosition == 'R') or (startingPosition == 'right'
and switchPosition == 'L'):
self.auton = autonFarSwitch(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'scale':
if (startingPosition == 'left'
and scalePosition == 'L') or (startingPosition == 'right'
and scalePosition == 'R'):
self.auton = autonNearScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and scalePosition == 'R') or (startingPosition == 'right'
and scalePosition == 'L'):
self.auton = autonFarScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'Two Cube Scale':
if (startingPosition == 'left'
and scalePosition == 'L') or (startingPosition == 'right'
and scalePosition == 'R'):
self.auton = autonTwoCubeSale(startingPosition, switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and scalePosition == 'R') or (startingPosition == 'right'
and scalePosition == 'L'):
self.auton = autonFarScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'drive':
self.auton = autonDrive(startingPosition, switchPosition,
scalePosition, self.drive)
print(self.auton)
def autonomousInit(self):
self.autonCounter = 0
self.drive.zeroGyro()
self.drive.resetMoveNumber()
self.drive.autonShift('low') #Forces into low gear at start of auton
#print('reset moveNumber')
self.interperetDashboard()
#self.auton = AutonInterpreter(3,3,3,self.drive)
#self.auton = autonTurningTuning('any', 'any', 'any', self.drive)
#self.auton = autonNearSwitch('right', 'R', 'L', self.drive)
#self.auton = autonFarSwitch('left', 'R', 'L', self.drive)
#self.auton = autonCenterEitherSwitch('center', 'L', 'L', self.drive)
#self.auton = autonCenterEitherSwitch('center', 'L', 'R', self.drive)
#self.auton = autonTwoCubeScale('left', 'L', 'L', self.drive)
#self.auton = autonNearScale('left', 'L', 'L', self.drive)
#self.auton = autonDrive('any', 'any', 'any', self.drive)
def autonomousPeriodic(self):
#self.drive.printEncoderPosition()#Prints the position of the encoders
#print(self.drive.getGyroAngle())
if self.autonCounter >= 5:
self.auton.run()
else:
self.autonCounter = self.autonCounter + 1
#self.AutonHandling.readCommandList(None, "square")
#lfEncoderPosition = -(self.drive.lfMotor.getQuadraturePosition())
#rbEncoderPosition = self.drive.rbMotor.getQuadraturePosition()
#averageEncoder = (lfEncoderPosition + rbEncoderPosition) / 2
#wpilib.SmartDashboard.putNumber('Left Encoder Position', lfEncoderPosition)
#wpilib.SmartDashboard.putNumber('Right Encoder Position', rbEncoderPosition)
#wpilib.SmartDashboard.putNumber(' Average Encodes', averageEncoder)
wpilib.SmartDashboard.putNumber('Gyro Angle',
self.drive.getGyroAngle())
#self.drive.printer()
def teleopPeriodic(self):
lfEncoderPosition = -(self.drive.lfMotor.getQuadraturePosition())
rfEncoderPosition = self.drive.rbMotor.getQuadraturePosition()
#self.drive.printEncoderPosition()
#print("Gyro Angle", self.drive.getGyroAngle())
wpilib.SmartDashboard.putNumber('Gyro Angle',
self.drive.getGyroAngle())
#wpilib.SmartDashboard.putNumber('Number of Shits', self.drive.shiftCounterReturn())
#wpilib.SmartDashboard.putString('Gear Mode', self.drive.gearMode())
#self.drive.printer()
self.drive.drivePass(self.controllerOne.getLeftY(),
self.controllerOne.getRightX(),
self.controllerOne.getLeftBumper(),
self.controllerOne.getRightBumper(),
self.controllerOne.getButtonA())
self.drive.operate.operate(self.controllerTwo.getLeftY(),
self.controllerTwo.getLeftX(),
self.controllerTwo.getRightY(),
self.controllerTwo.getRightX(),
self.controllerTwo.getButtonA(),
self.controllerTwo.getButtonB(),
self.controllerTwo.getButtonX(),
self.controllerTwo.getButtonY(),
self.controllerTwo.getRightTrigger(),
self.controllerTwo.getRightBumper(),
self.controllerTwo.getLeftTrigger(),
self.controllerTwo.getLeftBumper(),
self.controllerTwo.getStart(),
self.controllerTwo.getBack())
self.timeOut.time += 1
