def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# make this true to ignore joystick errors
self.debug = False
self.enabled_time = 0
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.navigation = Navigation(self)
self.pneumatics = Pneumatics(self)
self.peripherals = Peripherals(self)
#wpilib.SmartDashboard.putData(self.drivetrain)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
wpilib.SmartDashboard.putData(Scheduler.getInstance())
# instantiate the command used for the autonomous period
self.autonomousCommand = None
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
print("[Robot] Initializing")
logging.basicConfig(level=logging.DEBUG)
print("[Robot] Starting network tables server")
NetworkTables.initialize()
print("[Robot] Setting up systems")
self.cargosystem = CargoSystem(self)
self.climbsystem = ClimbSystem(self)
self.drivetrain = DriveTrain(self)
self.hatchsystem = HatchSystem(self)
self.oi = OI(self)
# setup autonomous type
print("[Robot] Setting up autonomous")
scAutonomousType = wpilib.SendableChooser()
scAutonomousType.setDefaultOption("1. Driver Controlled", 100)
scAutonomousType.addOption ("2. Timed Just Drive Forward", 200)
scAutonomousType.addOption ("3. Motion Profile Just Drive Forward", 300)
scAutonomousType.addOption ("4. Total Autonomous", 400)
wpilib.SmartDashboard.putData ("Anonomous Type", scAutonomousType)
# setup robot positions
scRobotPosition = wpilib.SendableChooser()
scRobotPosition.setDefaultOption("1. Upper Left" , 1)
scRobotPosition.addOption ("2. Lower Left" , 2)
scRobotPosition.addOption ("3. Centre" , 3)
scRobotPosition.addOption ("4. Upper Right", 4)
scRobotPosition.addOption ("5. Lower Right", 5)
wpilib.SmartDashboard.putData ("Robot Position", scRobotPosition)
# setup hatch positions
scHatchPosition = wpilib.SendableChooser()
scHatchPosition.setDefaultOption("1. Back" , 10)
scHatchPosition.addOption ("2. Middle" , 20)
scHatchPosition.addOption ("3. Front" , 30)
wpilib.SmartDashboard.putData ("Hatch Position", scHatchPosition)
# wpilib.CameraServer.launch("d:\\jet\\projects\\python\\RobotPy\\src\\vision.py:main")
# wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putNumber("H-Lower", 0)
wpilib.SmartDashboard.putNumber("H-Upper", 180)
wpilib.SmartDashboard.putNumber("S-Lower", 0)
wpilib.SmartDashboard.putNumber("S-Upper", 255)
wpilib.SmartDashboard.putNumber("V-Lower", 0)
wpilib.SmartDashboard.putNumber("V-Upper", 255)
# add HSV bounds to dashboard
def teleopInit(self):
return super().teleopInit()
def teleopPeriodic(self):
Scheduler.getInstance().run()
def log(self):
self.drivetrain.log()
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""This function is run when the robot is first started up and should be
used for any initialization code."""
self.drivetrain = DriveTrain(self)
self.elevator = Elevator(self)
self.wrist = Wrist(self)
self.claw = Claw()
self.oi = OI(self)
# instantiate the command used for the autonomous period
self.autonomousCommand = Autonomous(self)
# Show what command your subsystem is running on the SmartDashboard
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.elevator)
wpilib.SmartDashboard.putData(self.wrist)
wpilib.SmartDashboard.putData(self.claw)
wpilib.LiveWindow.getInstance().setEnabled(True)
def autonomousInit(self):
# schedule the autonomous command (example)
self.autonomousCommand.start()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous"""
Scheduler.getInstance().run()
self.log()
def teleopInit(self):
# This makes sure that the autonomous stops running when
# teleop starts running. If you want the autonomous to
# continue until interrupted by another command, remove
# this line or comment it out.
self.autonomousCommand.cancel()
def teleopPeriodic(self):
"""This function is called periodically during operator control"""
Scheduler.getInstance().run()
self.log()
def testPeriodic(self):
"""This function is called periodically during test mode"""
wpilib.LiveWindow.getInstance().updateValues()
def log(self):
"""The log method puts interesting information to the SmartDashboard."""
self.wrist.log()
self.elevator.log()
self.drivetrain.log()
self.claw.log()
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''This function is run when the robot is first started up and should be
used for any initialization code.'''
self.drivetrain = DriveTrain(self)
self.elevator = Elevator(self)
self.wrist = Wrist(self)
self.claw = Claw()
self.oi = OI(self)
# instantiate the command used for the autonomous period
self.autonomousCommand = Autonomous(self)
# Show what command your subsystem is running on the SmartDashboard
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.elevator)
wpilib.SmartDashboard.putData(self.wrist)
wpilib.SmartDashboard.putData(self.claw)
def autonomousInit(self):
# schedule the autonomous command (example)
self.autonomousCommand.start()
def autonomousPeriodic(self):
'''This function is called periodically during autonomous'''
Scheduler.getInstance().run()
self.log()
def teleopInit(self):
# This makes sure that the autonomous stops running when
# teleop starts running. If you want the autonomous to
# continue until interrupted by another command, remove
# this line or comment it out.
self.autonomousCommand.cancel()
def teleopPeriodic(self):
'''This function is called periodically during operator control'''
Scheduler.getInstance().run()
self.log()
def testPeriodic(self):
'''This function is called periodically during test mode'''
wpilib.LiveWindow.run()
def log(self):
'''The log method puts interesting information to the SmartDashboard.'''
self.wrist.log()
self.elevator.log()
self.drivetrain.log()
self.claw.log()
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
self.blocker = Blocker(self)
self.climbmotors = Climbmotors(self)
self.climbpistons = Climbpistons(self)
self.agitator = Agitator(self)
wpilib.SmartDashboard.putStringArray(
"Auto List",
["Far Left", "Far Right", "Center", "Center Low Goal", "Default"])
"""self.autoChooser.addOption("Far Left", AutoFarLeft)
self.autoChooser.addOption("Far Right", AutoFarRight)
self.autoChooser.addOption("Center", AutoShoot)
self.autoChooser.addOption("Center Low Goal", AutoCenterLowGoal)
self.autoChooser.setDefaultOption("Default", AutoBackupShoot)
wpilib.SmartDashboard.putData('Select Autonomous...', self.autoChooser)"""
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot", DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
# Define Robot getter
Command.getRobot = lambda: self
# Create subsystems
self.liftElevator = LiftElevator()
self.drivetrain = DriveTrain()
self.intakeOutput = IntakeOutput()
self.compressor = Compressor()
self.compressor.start()
# Shuffleboard options
# Autonomous commands
# self.autonomousCommand = DelayedDriveToBaseLine()
self.autonomousCommand = DriveToBaseLine()
"""
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
"""
self.oi = OI()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.drive = Drive()
self.stick = Joystick(0)
def robotInit(self):
self.activated = False
"""Robot-wide initialization code should go here."""
#SwiftCameraServer().launch('camera1.py:main')
#SwiftCameraServer().launch('camera2.py:main')
wpilib.CameraServer.launch()
self.configuration = constants.COMP_BOT
self.arm = Arm(self)
self.drivetrain = DriveTrain(self)
self.pneumatic = Pneumatic(self)
self.compressor = wpilib.Compressor()
self.camera_servo_1 = CameraServo(self, constants.CAMERA_1_PWM,
constants.CAMERA_1_DEFAULT)
self.camera_servo_2 = CameraServo(self, constants.CAMERA_2_PWM,
constants.CAMERA_2_DEFAULT)
self.oi = OI(self)
def robotInit(self):
"""This function is run when the robot is first started up and should be
used for any initialization code."""
self.drivetrain = DriveTrain(self)
self.elevator = Elevator(self)
self.wrist = Wrist(self)
self.claw = Claw()
self.oi = OI(self)
# instantiate the command used for the autonomous period
self.autonomousCommand = Autonomous(self)
# Show what command your subsystem is running on the SmartDashboard
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.elevator)
wpilib.SmartDashboard.putData(self.wrist)
wpilib.SmartDashboard.putData(self.claw)
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.puncher = Puncher()
self.claw = Claw()
self.arm = Arm()
self.intake = Intake()
self.elevator = Elevator()
self.hatch = Hatch()
self.intake_winch = IntakeWinch()
self.oi = OI(self)
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot",
DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.rear_puncher = RearPuncher()
self.arm = Arm()
self.intake = Intake()
self.elevator = Elevator()
self.hatch = Hatch()
self.intake_winch = IntakeWinch()
self.lift = Lift()
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
'''This function is run when the robot is first started up and should be
used for any initialization code.'''
self.drivetrain = DriveTrain(self)
self.elevator = Elevator(self)
self.wrist = Wrist(self)
self.claw = Claw()
self.oi = OI(self)
# instantiate the command used for the autonomous period
self.autonomousCommand = Autonomous(self)
# Show what command your subsystem is running on the SmartDashboard
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.elevator)
wpilib.SmartDashboard.putData(self.wrist)
wpilib.SmartDashboard.putData(self.claw)
def robotInit(self):
"""Robot-wide initialization code should go here"""
super().__init__()
if self.isReal(): # use the real drive train
self.drivetrain = DriveTrain(self)
else: # use the simulated drive train
#self.drivetrain = DriveTrainSim(self)
self.drivetrain = DriveTrainSim(self)
self.shooter = Shooter(self)
self.vision = Vision()
# oi MUST be created after all other subsystems since it uses them
self.oi = OI(self)
self.enabled_time = 0 # something is especially weird with the sim about this needing to be initialized in robotInit
self.autonomousCommand = None # initialize the placeholder command for autonomous
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
self.blocker = Blocker(self)
self.climbmotors = Climbmotors(self)
self.climbpistons = Climbpistons(self)
self.agitator = Agitator(self)
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.setDefaultOption("Drive", DriveAutonomous(self))
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.drive = Drive()
self.stick = Joystick(0)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.drive.moveToPosition(10000, 'left')
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.stick.getY() * -1
rotation = self.stick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
def testInit(self):
pass
def testPeriodic(self):
pass
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kElevator = robotmap.Elevator
self.kSticks = robotmap.Sticks
self.kClimber = robotmap.Climber
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = cubeGrabber(self)
self.elevator = Elevator(self)
self.climber = Climber(self)
self.sendableChooser()
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.autonomous = Autonomous(self)
self.autonomous.reset()
self.drive.autoInit()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#self.autonomous.testMove(self.autonomous.WALL_TO_SCALE, -1, False)
#self.autonomous.testAngle(-90, -1)
#self.elevator.setElevatorPosition(self.elevator.kScale)
#self.autonomous.start()
self.autonomous.run()
#self.elevator.setElevatorPosition(-20000)
#self.autonomous.telemetry()
def teleopInit(self):
self.drive.teleInit()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = (self.dStick.getY() * -1)**3
rotation = self.dStick.getTwist()/(1.1+self.dStick.getRawAxis(3))
# self.drive.moveSpeed(speed, speed)
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
#
self.elevator.elevatorFunction()
#self.elevator.telemetry()
self.climber.climberFunction()
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
class Robot(CommandBasedRobot):
"""This is the main class for running the PacGoat code."""
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# make this true to ignore joystick errors
self.debug = False
self.enabled_time = 0
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.navigation = Navigation(self)
self.pneumatics = Pneumatics(self)
self.peripherals = Peripherals(self)
#wpilib.SmartDashboard.putData(self.drivetrain)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
wpilib.SmartDashboard.putData(Scheduler.getInstance())
# instantiate the command used for the autonomous period
self.autonomousCommand = None
def autonomousInit(self):
self.reset()
self.enabled_time = Timer.getFPGATimestamp()
# self.autonomousCommand = self.autoChooser.getSelected()
# self.autonomousCommand.start()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous"""
Scheduler.getInstance().run()
self.log()
def teleopInit(self):
"""This function is called at the beginning of operator control."""
# This makes sure that the autonomous stops running when
# teleop starts running. If you want the autonomous to
# continue until interrupted by another command, remove
# this line or comment it out.
self.reset()
self.enabled_time = Timer.getFPGATimestamp()
if self.autonomousCommand is not None:
self.autonomousCommand.cancel()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
Scheduler.getInstance().run()
self.log()
def testPeriodic(self):
"""This function is called periodically during test mode."""
#wpilib.LiveWindow.run()
def disabledInit(self):
self.reset()
# self.shooter.unlatch()
def disabledPeriodic(self):
"""This function is called periodically while disabled."""
self.log()
def log(self):
if self.isReal():
self.drivetrain.log()
self.navigation.log()
def reset(self):
self.drivetrain.reset()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# TODO: see examples/pacgoat/robot.py
# we need Classes defining autonomous commands to call for Forward
# and Reverse
#self.autoChooser = wpilib.SendableChooser()
#self.autoChooser.addDefault("Forward", Forward(self))
#self.autoChooser.addObject("Reverse", Reverse(self))
#wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#nearswitch, scale, farswitch = list(self.fielddata)
#
# if nearswitch == 'R':
# self.drive.arcade(.5, .2)
# else:
# self.drive.arcade(.5, -.2)
pass
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
if self.isSimulation():
self.drive.arcade(speed, rotation)
else:
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
# TODO: need something like this in commands that autonomous or teleop
# would run to make sure an exception doesn't crash the code during
# competition.
#raise ValueError("Checking to see what happens when we get an exception")
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
class Robot(wpilib.IterativeRobot):
"""This is the main class for running the PacGoat code."""
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot",
DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def autonomousInit(self):
self.autonomousCommand = self.autoChooser.getSelected()
self.autonomousCommand.start()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous"""
Scheduler.getInstance().run()
self.log()
def teleopInit(self):
"""This function is called at the beginning of operator control."""
#This makes sure that the autonomous stops running when
#teleop starts running. If you want the autonomous to
#continue until interrupted by another command, remove
#this line or comment it out.
if self.autonomousCommand is not None:
self.autonomousCommand.cancel()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
Scheduler.getInstance().run()
self.log()
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()
def disabledInit(self):
self.shooter.unlatch()
def disabledPeriodic(self):
"""This function is called periodically while disabled."""
self.log()
def log(self):
self.pneumatics.writePressure()
wpilib.SmartDashboard.putNumber("Pivot Pot Value",
self.pivot.getAngle())
wpilib.SmartDashboard.putNumber(
"Left Distance",
self.drivetrain.getLeftEncoder().getDistance())
wpilib.SmartDashboard.putNumber(
"Right Distance",
self.drivetrain.getRightEncoder().getDistance())
class Robot(wpilib.IterativeRobot):
"""This is the main class for running the PacGoat code."""
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot", DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def autonomousInit(self):
self.autonomousCommand = self.autoChooser.getSelected()
self.autonomousCommand.start()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous"""
Scheduler.getInstance().run()
self.log()
def teleopInit(self):
"""This function is called at the beginning of operator control."""
#This makes sure that the autonomous stops running when
#teleop starts running. If you want the autonomous to
#continue until interrupted by another command, remove
#this line or comment it out.
if self.autonomousCommand is not None:
self.autonomousCommand.cancel()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
Scheduler.getInstance().run()
self.log()
def testPeriodic(self):
"""This function is called periodically during test mode."""
wpilib.LiveWindow.run()
def disabledInit(self):
self.shooter.unlatch()
def disabledPeriodic(self):
"""This function is called periodically while disabled."""
self.log()
def log(self):
self.pneumatics.writePressure()
wpilib.SmartDashboard.putNumber("Pivot Pot Value", self.pivot.getAngle())
wpilib.SmartDashboard.putNumber("Left Distance", self.drivetrain.getLeftEncoder().getDistance())
wpilib.SmartDashboard.putNumber("Right Distance", self.drivetrain.getRightEncoder().getDistance())
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# holds data from the FMS about the field elements.
self.fielddata = None
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data -- currently removed until we add an IF statement
# self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
## get field data
#if not self.fielddata:
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
#nearswitch, scale, farswitch = list(self.fielddata)
#if self.fielddata[0] == 'R':
#self.drive.arcade(.5, .2)
#else:
#self.drive.arcade(.5, -.2)
self.drive.arcade(.5, 0)
def teleopInit(self):
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
self.cubeGrabber.grabberFunction()
def testInit(self):
pass
def testPeriodic(self):
pass
class MyRobot(wpilib.TimedRobot):
"""Main robot class."""
def robotInit(self):
self.activated = False
"""Robot-wide initialization code should go here."""
#SwiftCameraServer().launch('camera1.py:main')
#SwiftCameraServer().launch('camera2.py:main')
wpilib.CameraServer.launch()
self.configuration = constants.COMP_BOT
self.arm = Arm(self)
self.drivetrain = DriveTrain(self)
self.pneumatic = Pneumatic(self)
self.compressor = wpilib.Compressor()
self.camera_servo_1 = CameraServo(self, constants.CAMERA_1_PWM,
constants.CAMERA_1_DEFAULT)
self.camera_servo_2 = CameraServo(self, constants.CAMERA_2_PWM,
constants.CAMERA_2_DEFAULT)
self.oi = OI(self)
def robotPeriodic(self):
if constants.SEND_DATA:
self.putData()
def autonomousInit(self):
"""Called only at the beginning of autonomous mode."""
if not self.activated:
self.activeInit()
def autonomousPeriodic(self):
self.activePeriodic()
def disabledInit(self):
self.activated = False
def disabledPeriodic(self):
"""Called every 20ms in disabled mode."""
def teleopInit(self):
if not self.activated:
self.activeInit()
def activeInit(self):
self.activated = True
self.arm.wrist.pid.reset()
self.arm.wrist.encoder.setPosition(constants.WRIST_START_POSITION)
self.arm.wrist.pid.setSetpoint(constants.WRIST_START_POSITION)
self.arm.wrist.pid.setEnabled(True)
self.arm.elbow.pid.reset()
self.arm.elbow.encoder.setPosition(constants.ELBOW_START_POSITION)
self.arm.elbow.pid.setSetpoint(constants.ELBOW_START_POSITION)
self.arm.elbow.pid.setEnabled(True)
self.arm.shoulder.pid.reset()
self.arm.shoulder.encoder.setPosition(
constants.SHOULDER_START_POSITION)
self.arm.shoulder.pid.setSetpoint(constants.SHOULDER_START_POSITION)
self.arm.shoulder.pid.setEnabled(True)
if constants.DRIVETRAIN_USE_PID:
self.drivetrain.resetPID()
self.drivetrain.enablePID()
#self.compressor.stop()
self.pneumatic.lift.solenoid.set(
self.pneumatic.lift.solenoid.Value.kForward)
self.pneumatic.hatch.solenoid.set(
self.pneumatic.hatch.solenoid.Value.kOff)
def teleopPeriodic(self):
self.activePeriodic()
def activePeriodic(self):
"""Called every 20ms in teleoperated mode"""
# Print out the num ber of loop iterations passed every second
Scheduler.getInstance().run()
# begin drive
y_axis_drive_input = self.oi.joy1.getY()
x_axis_drive_input = self.oi.joy1.getRawAxis(4)
if constants.DRIVETRAIN_USE_PID:
self.drivetrain.drive.setSafetyEnabled(False)
self.drivetrain.arcadeDrive(
y_axis_drive_input, x_axis_drive_input * -1 *
constants.DRIVE_INPUT_X_AXIS_MULTIPLIER)
else:
self.drivetrain.drive.arcadeDrive(
constants.DRIVE_INPUT_Y_AXIS_MULTIPLIER *
(y_axis_drive_input * -1),
x_axis_drive_input * constants.DRIVE_INPUT_X_AXIS_MULTIPLIER)
# end drive
shoulderAngle = self.arm.shoulder.pid.getSetpoint()
elbowAngle = self.arm.elbow.pid.getSetpoint()
# begin shoulder
axisValue = self.oi.joy2.getRawAxis(1)
print("Shoulder axis value:" + str(axisValue))
if abs(axisValue) >= .05:
pass
else:
axisValue = 0
if self.oi.joy2.getRawButton(5) and self.oi.joy2.getRawButton(6):
self.arm.shoulder.encoder.setPosition(
constants.SHOULDER_SETPOINT_MIN)
self.arm.shoulder.pid.setSetpoint(constants.SHOULDER_SETPOINT_MIN)
#if (elbowAngle > constants.MOVE_SHOULDER_ELBOW_MAX or elbowAngle < constants.MOVE_SHOULDER_ELBOW_MIN) or shoulderAngle > constants.MOVE_ELBOW_SHOULDER_MIN:
newSetpoint = shoulderAngle + (
(axisValue * -1) * constants.SHOULDER_MAX_DEGREES_PER_SECOND)
override = self.oi.joy2.getRawButton(5) or self.oi.joy2.getRawButton(6)
fixingSetpoint = (shoulderAngle >= constants.SHOULDER_SETPOINT_MAX
and newSetpoint < shoulderAngle) or (
shoulderAngle <= constants.SHOULDER_SETPOINT_MIN
and newSetpoint > shoulderAngle)
if fixingSetpoint or override or (
newSetpoint >= constants.SHOULDER_SETPOINT_MIN
and newSetpoint <= constants.SHOULDER_SETPOINT_MAX):
self.arm.shoulder.pid.setSetpoint(newSetpoint)
self.pneumatic.suspension.assist(axisValue)
#else:
# pass
# end shoulder
# begin elbow
axisValue = self.oi.joy2.getRawAxis(5)
print("Elbow axis value:" + str(axisValue))
if abs(axisValue) >= .05:
pass
else:
axisValue = 0
#if shoulderAngle < constants.MOVE_ELBOW_SHOULDER_MIN and (elbowAngle < constants.MOVE_SHOULDER_ELBOW_MAX and elbowAngle > constants.MOVE_SHOULDER_ELBOW_MIN):
# pass
#else:
self.arm.elbow.pid.setSetpoint(elbowAngle + (
(axisValue * -1) * constants.ELBOW_MAX_DEGREES_PER_SECOND))
# end elbow
# begin wrist
value = self.arm.wrist.pid.getSetpoint()
controlValue = (
-1 * self.oi.joy2.getRawAxis(3)) + self.oi.joy2.getRawAxis(2)
self.arm.wrist.pid.setSetpoint(
value + (controlValue * constants.WRIST_MAX_DEGREES_PER_SECOND))
# end wrist
if self.oi.joy1_btn_b.get():
self.pneumatic.lift.extend()
#self.pneumatic.suspension.extend()
elif self.oi.joy1_btn_a.get():
self.pneumatic.lift.retract()
#self.pneumatic.suspension.retract()
elif self.oi.joy2_btn_b.get():
#self.pneumatic.hatch.extend()
pass
elif self.oi.joy2_btn_a.get():
#self.pneumatic.hatch.retract()
pass
def putData(self):
wpilib.SmartDashboard.putNumber("Arm Length", self.arm.getArmLength())
wpilib.SmartDashboard.putNumber("Shoulder Encoder",
self.arm.shoulder.encoder.get())
wpilib.SmartDashboard.putNumber(
"Shoulder Encoder Direction",
self.arm.shoulder.encoder.getDirection())
wpilib.SmartDashboard.putNumber("Shoulder Encoder Rate",
self.arm.shoulder.encoder.getRate())
wpilib.SmartDashboard.putBoolean("Shoulder PID on Target",
self.arm.shoulder.pid.onTarget())
wpilib.SmartDashboard.putNumber("Shoulder PID Error",
self.arm.shoulder.pid.getError())
wpilib.SmartDashboard.putNumber("Shoulder PID Set Point",
self.arm.shoulder.pid.getSetpoint())
wpilib.SmartDashboard.putNumber("Shoulder PWM",
self.arm.shoulder.motors.get())
wpilib.SmartDashboard.putNumber(
"Shoulder Encoder Angle", self.arm.shoulder.encoder.getDistance())
wpilib.SmartDashboard.putNumber("Elbow Encoder",
self.arm.elbow.encoder.get())
wpilib.SmartDashboard.putNumber("Elbow Encoder Direction",
self.arm.elbow.encoder.getDirection())
wpilib.SmartDashboard.putNumber("Elbow Encoder Rate",
self.arm.elbow.encoder.getRate())
wpilib.SmartDashboard.putBoolean("Elbow PID on Target",
self.arm.elbow.pid.onTarget())
wpilib.SmartDashboard.putNumber("Elbow PID Error",
self.arm.elbow.pid.getError())
wpilib.SmartDashboard.putNumber("Elbow PID Set Point",
self.arm.elbow.pid.getSetpoint())
wpilib.SmartDashboard.putNumber("Elbow PWM",
self.arm.elbow.motors.get())
wpilib.SmartDashboard.putNumber("Elbow Encoder Angle",
self.arm.elbow.encoder.getDistance())
wpilib.SmartDashboard.putNumber("Wrist Encoder",
self.arm.wrist.encoder.get())
wpilib.SmartDashboard.putNumber("Wrist Encoder Direction",
self.arm.wrist.encoder.getDirection())
wpilib.SmartDashboard.putNumber("Wrist Encoder Rate",
self.arm.wrist.encoder.getRate())
wpilib.SmartDashboard.putBoolean("Wrist PID on Target",
self.arm.wrist.pid.onTarget())
wpilib.SmartDashboard.putNumber("Wrist PID Error",
self.arm.wrist.pid.getError())
wpilib.SmartDashboard.putNumber("Wrist PID Set Point",
self.arm.wrist.pid.getSetpoint())
wpilib.SmartDashboard.putNumber("Wrist PWM",
self.arm.wrist.wristMotor.get())
wpilib.SmartDashboard.putNumber("Wrist Encoder Angle",
self.arm.wrist.encoder.getDistance())
wpilib.SmartDashboard.putNumber(
"Left Shoulder Temp:",
(self.arm.shoulder.leftMotor.getMotorTemperature() * 9 / 5) + 32)
wpilib.SmartDashboard.putNumber(
"Right Shoulder Temp:",
(self.arm.shoulder.rightMotor.getMotorTemperature() * 9 / 5) + 32)
wpilib.SmartDashboard.putNumber(
"Left Elbow Temp:",
(self.arm.elbow.leftMotor.getMotorTemperature() * 9 / 5) + 32)
wpilib.SmartDashboard.putNumber(
"Right Elbow Temp:",
(self.arm.elbow.rightMotor.getMotorTemperature() * 9 / 5) + 32)
wpilib.SmartDashboard.putNumber(
"Wrist Temp:",
(self.arm.wrist.wristMotor.getMotorTemperature() * 9 / 5) + 32)
wpilib.SmartDashboard.putNumber(
"Battery Voltage:", wpilib.RobotController.getBatteryVoltage())
