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):
#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 __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):
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, dist):
Command.__init__(self)
self.robot = robot
self.dist = dist
self.s = 0
self.i = 0
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, 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, 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, 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):
Command.__init__(self, name='DriveToBall')
self.robot = robot
self.vision = robot.vision
self.drivetrain = robot.drivetrain
self.requires(self.drivetrain)
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 __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 __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 __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 __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, robot):
# Recognize as a wpilib command
print("do_big_climb init!!")
Command.__init__(self)
self.requires(robot.climber)
self.climber = robot.climber
def __init__(self):
Command.__init__(self, "EmergencyStop")
self.requires(self.getRobot().drivetrain)
def __init__(self):
Command.__init__(self, "Follow Joystick")
self.requires(self.getRobot().drivetrain)
def __init__(self, robot):
Command.__init__(self)
print("do_carrier init")
self.requires(robot.carrier)
self.carrier = robot.carrier
def __init__(self):
Command.__init__(self, "Intake")
self.requires(self.getRobot().IntakeRoller)
def __init__(self):
Command.__init__(self, "ShootCell")
# Class variables
self.gate = GateShot_Subsystem()
self.orientation = 0
def __init__(self, robot):
print("do_intake init")
Command.__init__(self)
self.intake = robot.intake
def __init__(self):
Command.__init__(self, "SpinColorWheel")
# self.robot = self.getRobot()
self.motor = ColorSpinner()
def __init__(self, robot):
print("Do_Bad_Auto init!!")
Command.__init__(self)
self.requires(robot.drivetrain)
self.drivetrain = robot.drivetrain
def __init__(self, robot):
Command.__init__(self, name='shooter_hood_axis')
# self.requires(robot.shooter)
self.robot = robot
self.hood_scale = 0.2
self.hood_offset = 0.0