def flowcontrolWHILE(func):
parent = _getCommandGroup()
source = _getSource(parent)
try:
parentLoop = source._currentLoop
except AttributeError:
parentLoop = None
cg = CommandGroup(func.__name__)
cg._source = source
# Set the current loop for any BREAK statements
source._currentLoop = cg
func(cg)
source._currentLoop = parentLoop
def cancelLoop(self):
self.getGroup().forceCancel = True
end = Command("END WHILE")
end.initialize = cancelLoop.__get__(end)
cond = ConditionalCommand("flowcontrolWHILE", cg, end)
cond.condition = condition
cond.forceCancel = False
cond.isFinished = _restartWhile.__get__(cond)
cond._parentLoop = parentLoop
parent.addSequential(cond)
cg.conditionalCommand = cond
class robot(wpilib.IterativeRobot):
robot_map = robotmap()
robot_drive = robot_drive_system()
ctrl = oi.ps4_ctrl
def robotInit(self):
self.autocommand = Command()
def autonomousInit(self):
super().autonomousInit()
def teleopInit(self):
if self.autocommand:
self.autocommand.cancel()
drive_by_controller()
def testInit(self):
super().testInit()
def autonomousPeriodic(self):
super().autonomousPeriodic()
def teleopPeriodic(self):
super().teleopPeriodic()
def testPeriodic(self):
super().testPeriodic()
def __init__(self,
robot,
setpoint=None,
timeout=None,
source=None,
absolute=False):
"""The constructor"""
Command.__init__(self, name='auto_rotate')
self.requires(robot.drivetrain)
self.setpoint = setpoint
self.source = source # sent directly to command or via dashboard
self.absolute = absolute # use a relative turn or absolute
if timeout is None:
self.timeout = 5
else:
self.timeout = timeout
self.setTimeout(self.timeout)
self.robot = robot
self.tolerance = 1
self.kp = 0.2
self.kd = 0.1
self.kf = 0.1
self.start_angle = 0
self.power = 0
self.max_power = 0.5 # clamp maximum power for turning so we don't over turn
self.error = 0
self.prev_error = 0
def __init__(self,
robot,
path: str,
relative=False,
reset_telemetry=True,
timeout=50):
Command.__init__(self, name='auto_rams_simple')
self.robot = robot
self.requires(robot.drivetrain)
self.setTimeout(timeout)
self.previous_time = -1
self.previous_speeds = None
self.use_PID = True
self.counter = 0
self.trajectory = None
self.relative = relative
self.path = path
self.reset_telemetry = reset_telemetry
self.dash = True
self.feed_forward = drive_constants.feed_forward
self.kinematics = drive_constants.drive_kinematics
self.course = drive_constants.course
def __init__(self, robot, dist):
Command.__init__(self)
self.robot = robot
self.dist = dist
self.s = 0
self.i = 0
def __init__(self):
#super().__init__("StopShooting")
Command.__init__(self, "StopIntake")
#tells the robot that the shooter is running, and that nothing else should run on the shooter.
self.requires(self.getRobot().intakeRoller)
def flowcontrolWHILE(func):
parent = _getCommandGroup()
source = _getSource(parent)
try:
parentLoop = source._currentLoop
except AttributeError:
parentLoop = None
cg = CommandGroup(func.__name__)
cg._source = source
# Set the current loop for any BREAK statements
source._currentLoop = cg
func(cg)
source._currentLoop = parentLoop
def cancelLoop(self):
self.getGroup().forceCancel = True
end = Command("END WHILE")
end.initialize = cancelLoop.__get__(end)
cond = ConditionalCommand("flowcontrolWHILE", cg, end)
cond.condition = condition
cond.forceCancel = False
cond.isFinished = _restartWhile.__get__(cond)
cond._parentLoop = parentLoop
parent.addSequential(cond)
cg.conditionalCommand = cond
def __init__(self):
# Create Joysticks
self._driveJoy = XboxController(0)
self._cyController = Joystick(1)
# Create Buttons
self._blastenTheHatches = JoystickButton(self._driveJoy, 2)
self._servoOpen = JoystickButton(self._driveJoy, 4)
self._servoHalf = JoystickButton(self._driveJoy, 3)
self._servoClose = JoystickButton(self._driveJoy, 1)
self._suplexButton = JoystickButton(self._driveJoy, 8)
self._backButton = JoystickButton(self._driveJoy, 7)
# Testing
self._moveMastUpButton = JoystickButton(self._cyController, 8)
self._moveMastDownButton = JoystickButton(self._cyController, 9)
# Connect Buttons to Commands
hatchEffector = Command.getRobot().hatchEffector
self._blastenTheHatches.whileHeld(hatchEffector.ParallelShoot(hatchEffector))
self._servoOpen.whileHeld(hatchEffector.ServoOpen(hatchEffector))
self._servoHalf.whileHeld(hatchEffector.ServoHalf(hatchEffector))
self._servoClose.whileHeld(hatchEffector.ServoClose(hatchEffector))
suplex = Command.getRobot().suplex
self._suplexButton.whileHeld(suplex.Smash(suplex, Flipper.FlipDirection.UP))
self._backButton.whileHeld(suplex.Smash(suplex, Flipper.FlipDirection.DOWN))
mastyBoi = Command.getRobot().mastyBoi
self._moveMastUpButton.whileHeld(mastyBoi.HoistTheColors(mastyBoi))
self._moveMastDownButton.whileHeld(mastyBoi.RetrieveTheColors(mastyBoi))
def __init__(self, robot):
Command.__init__(self)
self.robot = robot # access robot values
self.controller = robot.oi.getMainController() # get controller class
self.sideCon = robot.oi.getSideController(
) # get side controller class
def __init__(self, robot, button, end_condition='stop', angle=45, other_parameter=None):
Command.__init__(self, name='shooter_hood')
# self.requires(robot.shooter)
self.robot = robot
self.button = button
self.end_condition = end_condition
self.angle = angle
self.velocity = 1000 # rpm for the shooter
def __init__(self, robot):
Command.__init__(self, name='DriveToBall')
self.robot = robot
self.vision = robot.vision
self.drivetrain = robot.drivetrain
self.requires(self.drivetrain)
def __init__(self, robot, button):
Command.__init__(self, name='DpadDrive')
self.requires(robot.drivetrain)
self.robot = robot
self.button = button
self.mode = None
self.scale = 0.3
self.twist_scale = 0.35
def __init__(self, robot, button, timeout=60):
Command.__init__(self, name='frc_characterization')
self.requires(robot.drivetrain)
self.robot = robot
self.button = button
self.timeout = timeout
self.setTimeout(self.timeout)
def __init__(self, robot, dist, speed, angle):
Command.__init__(self)
self.robot = robot
self.dist = dist
self.speed = speed
self.angle = angle
self.i = 0
self.s = 0
def __init__(self):
Command.__init__(self, "AimBot")
self.requires(self.getRobot().drivetrain)
self.chamelion = self.getRobot().networkTable.getSubTable(
"chamelion-vision/USB Camera-B4.09.24.1")
self.aimYawTarget = 0
self.aimYawP = 0.01
# self.aimYawD = 0.2
self.aimPitchTarget = 0
self.aimPitchP = 0.005
def __init__(self):
Command.__init__(self, "Sushi_Act")
self.robot = self.getRobot()
#self.sushiWheel = self.getSushiWheel()
self.requires(self.getRobot().sushiWheel)
#self.requires(self.sushiWheel)
self.rpm = 0.25
self.toggle = False
def __init__(self):
Command.__init__(self, "Drive")
self.oi = self.getOi()
self.robot = self.getRobot()
self.speed = 0
self.rotation = 0
# self.controller = MasterController()
# self.driveTrain = DriveTrain()
self.requires(self.getRobot().driveTrain)
self.setInterruptible(True)
def __init__(self, robot):
print("do_tank_drive init")
Command.__init__(self)
# an instance of BeaverTronicsRobot from robot.py containing its
# instance of drivetrain
self.robot_dt = robot.drivetrain
self.requires(self.robot_dt)
self.robot = robot
self.left_joy = robot.left_joy
self.right_joy = robot.right_joy
def _printDriveType(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self.logger.info("WPI Mecanum Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self.logger.info("Morpheus Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self.logger.info("Field Oriented Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self.logger.info("No Joy Drive selected")
def execute(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self._spootnikDrives.driveCartesianWithJoy()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self._spootnikDrives.morpheusDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self._spootnikDrives.fieldOrientedDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self._spootnikDrives.noJoyDrive()
def __init__(self, robot, timeout=None):
"""The constructor"""
#super().__init__()
Command.__init__(self, name='auto_drivetimed')
# Signal that we require ExampleSubsystem
self.requires(robot.drivetrain)
if timeout is None:
self.timeout = 2
else:
self.timeout = timeout
self.setTimeout(self.timeout)
self.robot = robot
def __init__(self, robot):
print("do_shifters_toggle init")
Command.__init__(self)
self.robot_shifters = robot.shifters
# uses solenoids 0 and 1
'''
Shifters can onlu be in two states:
1: actuated(high gear)
2: unactuated(low gear)
States 1 & 2 (alternating): requires Shifters
'''
self.requires(self.robot_shifters)
def __init__(self, robot, timeout=50):
Command.__init__(self, name='auto_ramsete')
self.robot = robot
self.requires(robot.drivetrain)
self.setTimeout(timeout)
self.previous_time = -1
self.previous_speeds = None
self.use_PID = True
self.counter = 0
self.telemetry = []
self.trajectory = None
self.feed_forward = drive_constants.feed_forward
self.kinematics = drive_constants.drive_kinematics
self.course = drive_constants.course
def execute(self):
left_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kLeft)
right_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kRight)
left_trigger = self.apply_deadband(left_trigger)
right_trigger = self.apply_deadband(right_trigger)
# self.logger.info("Left trigger: {}".format(left_trigger))
# self.logger.info("Right trigger: {}".format(right_trigger))
if left_trigger > 0:
self._cargo_effector.cargo_motor.set(left_trigger)
else:
self._cargo_effector.cargo_motor.set(-right_trigger)
def __init__(self, left, right, timeoutInSeconds):
super().__init__('Set Speed %d and rotation %d' % (left, right),
timeoutInSeconds)
self.left = left
self.right = right
self.requires(Command.getRobot().drivetrain)
print("Created timed command")
def __init__(self, spootnikDrives):
super().__init__("DriveWithJoy")
self.logger = logging.getLogger("DriveWithJoy")
self.requires(spootnikDrives)
self._spootnikDrives = spootnikDrives
self._driveSwitcherVal = Command.getRobot(
).driveSwitcher.getSelected()
self._printDriveType()
def __init__(self, robot, setpoint=None, timeout=None, source=None):
"""The constructor"""
#super().__init__()
Command.__init__(self, name='auto_pid')
# Signal that we require ExampleSubsystem
self.requires(robot.drivetrain)
self.source = source
self.k_dash = False
self.start_time = 0
self.teaching = True # switch to True if you want to learn about PID controllers
# allow setpoint to be controlled by the dashboard
if source == 'dashboard':
setpoint = SmartDashboard.getNumber('z_distance', 1)
self.k_dash = True
else:
if setpoint is None:
setpoint = 2
# wpilib PID controller apparently does not like negative setpoints, so use this to allow going backwards
self.setpoint_sign = math.copysign(1, setpoint)
self.setpoint = math.fabs(setpoint) # correction for overshoot
if timeout is None:
self.timeout = 5
else:
self.timeout = timeout
self.setTimeout(timeout)
self.robot = robot
self.has_finished = False
self.error = 0
# using these k-values
self.kp = 1.8
self.kd = 4.0
self.ki = 0.00
self.kperiod = 0.1
self.tolerance = 0.1
if self.k_dash:
SmartDashboard.putNumber('zdrive_kp', self.kp)
SmartDashboard.putNumber('zdrive_ki', self.ki)
SmartDashboard.putNumber('zdrive_kd', self.kd)
SmartDashboard.putNumber('zdrive_per', self.kperiod)
def BREAK(steps=1):
"""
Calling this function will end the loop that contains it. Pass an integer to
break out of that number of nested loops.
"""
if steps < 1:
raise ValueError('Steps to BREAK cannot be < 1')
parent = _getCommandGroup()
source = _getSource(parent)
try:
loop = source._currentLoop
except AttributeError:
raise ValueError('Cannot BREAK outside of a loop')
if loop is None:
raise ValueError('Cannot BREAK outside of a loop')
# We can't use CancelCommand here, because the conditionalCommand attribute
# isn't yet bound to the CommandGroup, so we find it at initialization time
# instead
def cancelLoop():
nonlocal loop, steps
step = 1
while steps > step:
loop = loop.conditionalCommand._parentLoop
if loop is None:
raise ValueError(
'BREAK %i not possible with loop depth %i' %
(steps, step)
)
step += 1
loop.conditionalCommand.forceCancel = True
breakLoop = Command('flowcontrolBREAK')
breakLoop.initialize = cancelLoop
parent.addSequential(breakLoop)
def __init__(self, robot, left_speed_setpoint=1.0, right_speed_setpoint=-1, timeout=6):
Command.__init__(self, name='auto_velocity')
self.robot = robot
self.requires(robot.drivetrain)
self.setTimeout(timeout)
self.use_dash = True
# initialize the feed forward so we can use velocities
self.feed_forward = wpilib.controller.SimpleMotorFeedforwardMeters(
drive_constants.ks_volts, drive_constants.kv_volt_seconds_per_meter, drive_constants.ka_volt_seconds_squared_per_meter)
self.kp = 0.0
self.kd = 0.0
self.left_speed_setpoint = left_speed_setpoint
self.right_speed_setpoint = right_speed_setpoint
if self.use_dash:
SmartDashboard.putNumber('l_vel', self.left_speed_setpoint)
SmartDashboard.putNumber('r_vel', self.right_speed_setpoint)
SmartDashboard.putNumber('kp_vel', self.kp)
SmartDashboard.putNumber('kd_vel', self.kd)
def BREAK(steps=1):
"""
Calling this function will end the loop that contains it. Pass an integer to
break out of that number of nested loops.
"""
if steps < 1:
raise ValueError("Steps to BREAK cannot be < 1")
parent = _getCommandGroup()
source = _getSource(parent)
try:
loop = source._currentLoop
except AttributeError:
raise ValueError("Cannot BREAK outside of a loop")
if loop is None:
raise ValueError("Cannot BREAK outside of a loop")
# We can't use CancelCommand here, because the conditionalCommand attribute
# isn't yet bound to the CommandGroup, so we find it at initialization time
# instead
def cancelLoop():
nonlocal loop, steps
step = 1
while steps > step:
loop = loop.conditionalCommand._parentLoop
if loop is None:
raise ValueError("BREAK %i not possible with loop depth %i" %
(steps, step))
step += 1
loop.conditionalCommand.forceCancel = True
breakLoop = Command("flowcontrolBREAK")
breakLoop.initialize = cancelLoop
parent.addSequential(breakLoop)
def __init__(self):
Command.__init__(self)
self.requires(rev.color.ColorSensorV3)
self.totalRad = 0
self.totalRot = 0
self.vals = {
"Proximity": vs.Sensor.getProx,
"Red": vs.Sensor.getRed,
"Green": vs.Sensor.getGreen,
"Blue": vs.Sensor.getBlue
}
self.dash = wpilib.SmartDashboard
'''self.color = [red, green, blue]'''
self.rgbRed = [.42, .37, .25]
self.rgbGreen = [.19, .52, .25]
self.rgbBlue = [.15, .45, .34]
self.rgbYellow = [.32, .52, .12]
# Tolerance that values can comfortably fall within
self.offset = .05
self.isColor = False
self.bal = 0
def __init__(self):
Command.__init__(self, "EmergencyStop")
self.requires(self.getRobot().drivetrain)
