class Lift(Subsystem):
def __init__(self):
super().__init__()
self.front_lift = DoubleSolenoid(0, 6, 7)
self.rear_lift = wpilib.DoubleSolenoid(0, 2, 3)
def lift_front(self):
self.front_lift.set(DoubleSolenoid.Value.kReverse)
print("front lifted")
def lift_rear(self):
self.rear_lift.set(DoubleSolenoid.Value.kForward)
print("rear lifted")
def lower_rear(self):
self.rear_lift.set(DoubleSolenoid.Value.kReverse)
print("rear lowered")
def lower_front(self):
self.front_lift.set(DoubleSolenoid.Value.kForward)
print("front lowered")
def stop(self):
"""Turn off the solenoid."""
self.rear_lift.set(DoubleSolenoid.Value.kOff)
class DriverComponent(Events):
def __init__(self):
self.left_front = Talon()
self.left_rear = Talon()
self.right_front = Talon()
self.right_rear = Talon()
self.gear_solenoid = DoubleSolenoid()
#self.driver_gyro = ADXRS450_Gyro()
self._create_event(DriverComponent.EVENTS.driving)
def set_curve(self, linear, angular):
sf = abs(linear) + abs(angular)
self.left_front.set((linear + angular) / sf)
self.right_front.set((linear - angular) / sf)
self.right_rear.set((linear - angular) / sf)
self.left_rear.set((linear + angular) / sf)
def toggle_gear(self):
if self.current_gear() is GearMode.LOW:
self.set_high_gear()
if self.current_gear() is GearMode.HIGH:
self.set_low_gear()
def set_low_gear(self):
print("shift low")
self.gear_solenoid.set(DoubleSolenoid.Value.kReverse)
def set_high_gear(self):
print("shift high")
self.gear_solenoid.set(DoubleSolenoid.Value.kForward)
def __init__(self):
super().__init__("Plate Piston")
self.robot = team3200.getRobot()
self.map = self.robot.map.pneumaticsMap
self.platePiston = DoubleSolenoid(self.map.pcmCan,
self.map.forwardChannel,
self.map.reverseChannel)
class Solenoid():
def __init__(self, _low: int, _high: int):
self.ds = DoubleSolenoid(_low, _high)
def set(self, _bool):
self.ds.set(
self.ds.Value.kForward if _bool else self.ds.Value.kReverse)
def __init__(self, initialState = 0):
#Loader.State.kClose
super().__init__('loader subsystem')
pneumatics = wpilib.command.Command.getRobot().robotMap.pneumatics
self.loaderSolenoid = DoubleSolenoid(pneumatics.pcmCAN, pneumatics.loader_open, pneumatics.loader_close)
#intialization: close the loader so we know where it begins at.
self.currentState=initialState
self.setLoader(initialState)
def __init__(self):
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
class GripperComponent(Events):
lift_positions = {
"up": 0.2,
# "middle": 197,
"down": 0.595
# "up": 2.2,
# "down": 4.0
}
class EVENTS(object):
gripper_started_moving = "gripper_started_moving"
gripper_started_moving_data = Command
def __init__(self):
Events.__init__(self)
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
# setup events
self._create_event(GripperComponent.EVENTS.gripper_started_moving)
def set_spread_state(self, spread: bool):
if spread:
self.solenoid.set(DoubleSolenoid.Value.kForward)
self._spread_state = True
else:
self.solenoid.set(DoubleSolenoid.Value.kReverse)
self._spread_state = False
def toggle_spread_state(self):
if self._spread_state is None:
self.set_spread_state(False)
else:
self.set_spread_state(not self._spread_state)
def set_motor_speeds(self, left: float, right: float):
self.left_motor.set(left)
self.right_motor.set(-right)
def set_lift_motor(self, speed):
print("grip_speed: {}".format(str(speed)))
self.lift_motor.set(speed)
def current_lift_state(self) -> str:
positions = [(key, position) for key, position in GripperComponent.lift_positions.items()]
return min(positions, key=lambda position: abs(self.pot.get() - position[1]))[0]
def __init__(self):
self.left_front = Talon()
self.left_rear = Talon()
self.right_front = Talon()
self.right_rear = Talon()
self.gear_solenoid = DoubleSolenoid()
#self.driver_gyro = ADXRS450_Gyro()
self._create_event(DriverComponent.EVENTS.driving)
class Solenoid:
compressor = Compressor()
PCM_ID = 20
def __init__(self, forward_port, reverse_port):
'''
Creates a Solenoid object.
Takes two arguments, forward_port, and reverse_port.
These are the port numbers on the PCM that each solenoid
in the doublesolenoid is wired to.
The doublesolenoid is the solenoid that can invert pushing and pulling
on an actuator.
'''
self.wpilib_solenoid = DoubleSolenoid(self.PCM_ID, forward_port,
reverse_port)
self.forward_port = forward_port
self.reverse_port = reverse_port
self.inverted = False
self.set_forward()
def set_forward(self):
self.set_wrapped_solenoid(Reverse if self.is_inverted() else Forward)
def set_reverse(self):
self.set_wrapped_solenoid(Forward if self.is_inverted() else Reverse)
def extend(self):
self.set_forward()
def retract(self):
self.set_reverse()
def set_invert(self, invert):
'''
If `invert` is true, this inverts the Solenoids
where forwards is reverse and vice versa.
If `invert` is false, it brings it back to its normal state.
'''
self.inverted = invert
def is_inverted(self):
return self.inverted
def set_wrapped_solenoid(self, value):
self.wpilib_solenoid.set(value)
def __init__(self):
self.lower_shooter = Talon(4)
self.upper_shooter = Talon(5)
self.intake = Relay(1)
self.launcher = DoubleSolenoid(4, 5)
self.lifter = DoubleSolenoid(2, 3)
self.should_lift = False
self.is_shooting = False
self.retract_launcher()
self.raise_lifter()
class Lift(Subsystem):
def __init__(self, can_id=0, channel_1=1, channel_2=2):
super().__init__()
self.solenoid = DoubleSolenoid(can_id, channel_1, channel_2)
def set(self, state):
self.solenoid.set(state)
def extend(self):
self.set(1)
def retract(self):
self.set(2)
def __init__(self):
Events.__init__(self)
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
# setup events
self._create_event(GripperComponent.EVENTS.gripper_started_moving)
def __init__(self):
self.left_front = Talon(2)
self.left_rear = Talon(3)
self.right_front = Talon(0)
self.right_rear = Talon(1)
self.gear_solenoid = DoubleSolenoid(6, 7)
self.high_gear_enabled = False
#self.driver_gyro = ADXRS450_Gyro()
self.left_front.setInverted(True)
self.left_rear.setInverted(True)
self.set_low_gear()
class DriverComponent(Events):
def __init__(self):
self.left_front = Talon(2)
self.left_rear = Talon(3)
self.right_front = Talon(0)
self.right_rear = Talon(1)
self.gear_solenoid = DoubleSolenoid(6, 7)
self.high_gear_enabled = False
#self.driver_gyro = ADXRS450_Gyro()
self.left_front.setInverted(True)
self.left_rear.setInverted(True)
self.set_low_gear()
#self._create_event(DriverComponent.EVENTS.driving)
def filter_deadband(self, value: float):
if -0.1 < value < 0.1:
return 0
else:
return value
def set_curve_raw(self, linear, angular):
pass
def set_curve(self, linear, angular):
l = self.filter_deadband(linear)
a = self.filter_deadband(angular)
sf = max(1, abs(l) + abs(a))
self.left_front.set((l + a) / sf)
self.right_front.set((l - a) / sf)
self.right_rear.set((l - a) / sf)
self.left_rear.set((l + a) / sf)
def toggle_gear(self):
self.high_gear_enabled = not self.high_gear_enabled
if self.high_gear_enabled:
self.set_high_gear()
else:
self.set_low_gear()
def set_low_gear(self):
print("shift low")
self.gear_solenoid.set(DoubleSolenoid.Value.kReverse)
def set_high_gear(self):
print("shift high")
self.gear_solenoid.set(DoubleSolenoid.Value.kForward)
class GripperComponent:
lift_positions = {
"up": 0.13,
"down": 0.54,
# "up": 0.2,
# "down": 0.595
# "up": 2.2,
# "down": 4.0
}
def __init__(self):
self.left_motor = Victor(0)
self.right_motor = Victor(1)
self.solenoid = DoubleSolenoid(2, 3)
self.lift_motor = Victor(4)
self.pot = AnalogPotentiometer(0)
# state
self._lift_state = None
self._spread_state = None
def set_spread_state(self, spread: bool):
if spread:
self.solenoid.set(DoubleSolenoid.Value.kForward)
self._spread_state = True
else:
self.solenoid.set(DoubleSolenoid.Value.kReverse)
self._spread_state = False
def toggle_spread_state(self):
if self._spread_state is None:
self.set_spread_state(False)
else:
self.set_spread_state(not self._spread_state)
def set_motor_speeds(self, left: float, right: float):
self.left_motor.set(left)
self.right_motor.set(-right)
def set_lift_motor(self, speed):
self.lift_motor.set(speed)
def current_lift_state(self) -> str:
positions = [(key, position) for key, position in GripperComponent.lift_positions.items()]
return min(positions, key=lambda position: abs(self.pot.get() - position[1]))[0]
class Pneumatics(Subsystem):
def __init__(self, robot):
super().__init__()
self.counter = 0
self.double_solenoid = DoubleSolenoid(0, 1)
self.compressor = Compressor(0)
self.compressor.setClosedLoopControl(True)
def actuate_solenoid(self, direction=None):
if direction == 'open':
self.double_solenoid.set(DoubleSolenoid.Value.kForward)
elif direction == 'close':
self.double_solenoid.set(DoubleSolenoid.Value.kReverse)
else:
pass
def log(self):
pass
def createPistons(self, pistonSpec):
piston = None
if pistonSpec['Type'] == 'Double':
piston = DoubleSolenoid(pistonSpec['portA'], pistonSpec['portB'])
elif pistonSpec['Type'] == 'single':
piston = Solenoid(pistonSpec['portA'])
else:
print("IDK your pistons")
return piston
def __init__(self):
super().__init__("Intake")
self.talon_left = Talon(0)
self.talon_right = Talon(1)
self.solenoid_lift = DoubleSolenoid(0, 1)
self.solenoid_grab = DoubleSolenoid(2, 3)
self.set_grab_state(GrabState.IN)
self.set_arm_state(ArmState.UP)
self.power = .75
self.in_sensor = AnalogInput(0)
dashboard2.add_graph("Ultrasonic", self.get_reported_distance)
self.pdp = PowerDistributionPanel()
self.l_channel = 0
self.r_channel = 1
class ShooterComponent:
def __init__(self):
self.lower_shooter = Talon(4)
self.upper_shooter = Talon(5)
self.intake = Relay(1)
self.launcher = DoubleSolenoid(4, 5)
self.lifter = DoubleSolenoid(2, 3)
self.should_lift = False
self.is_shooting = False
self.retract_launcher()
self.raise_lifter()
def is_busy(self):
return self.is_shooting
def set_active(self):
self.is_shooting = True
def set_inactive(self):
self.is_shooting = False
def enable_intake(self):
self.intake.set(Relay.Value.kReverse)
def disable_intake(self):
self.intake.set(Relay.Value.kOff)
def engage_launcher(self):
self.launcher.set(DoubleSolenoid.Value.kReverse)
def retract_launcher(self):
self.launcher.set(DoubleSolenoid.Value.kForward)
def toggle_lifter(self):
self.should_lift = not self.should_lift
if self.should_lift:
self.raise_lifter()
else:
self.lower_lifter()
def raise_lifter(self):
self.lifter.set(DoubleSolenoid.Value.kReverse)
def lower_lifter(self):
self.lifter.set(DoubleSolenoid.Value.kForward)
def shoot(self, speed: float):
self.lower_shooter.set(speed)
self.upper_shooter.set(speed)
def __init__(self, forward_port, reverse_port):
'''
Creates a Solenoid object.
Takes two arguments, forward_port, and reverse_port.
These are the port numbers on the PCM that each solenoid
in the doublesolenoid is wired to.
The doublesolenoid is the solenoid that can invert pushing and pulling
on an actuator.
'''
self.wpilib_solenoid = DoubleSolenoid(self.PCM_ID, forward_port,
reverse_port)
self.forward_port = forward_port
self.reverse_port = reverse_port
self.inverted = False
self.set_forward()
def __init__(self):
left_front = WPI_TalonSRX(6)
left_rear = WPI_TalonSRX(1)
right_front = WPI_TalonSRX(4)
right_rear = WPI_TalonSRX(7)
left = SpeedControllerGroup(left_front, left_rear)
right = SpeedControllerGroup(right_front, right_rear)
self.left_encoder_motor = left_rear
self.right_encoder_motor = right_front
self.gear_solenoid = DoubleSolenoid(0, 1)
self.driver_gyro = ADXRS450_Gyro()
self.drive_train = DifferentialDrive(left, right)
# setup encoders
self.left_encoder_motor.setSensorPhase(True)
self.drive_train.setDeadband(0.1)
self.moving_linear = [0] * DriverComponent.LINEAR_SAMPLE_RATE
self.moving_angular = [0] * DriverComponent.ANGULAR_SAMPLE_RATE
class Loader(Subsystem):
def __init__(self, initialState=0):
#Loader.State.kClose
super().__init__('loader subsystem')
pneumatics = wpilib.command.Command.getRobot().robotMap.pneumatics
self.loaderSolenoid = DoubleSolenoid(pneumatics.pcmCAN,
pneumatics.loader_open,
pneumatics.loader_close)
#intialization: close the loader so we know where it begins at.
self.currentState = initialState
self.setLoader(initialState)
def toggleLoader(self):
#if claw is closed then open
if self.currentState == self.State.kOpen:
loaderState = self.State.kClose
print("Opening Loader")
#else if claw is open or not set then close
else:
loaderState = self.State.kOpen
self.setLoader(loaderState)
#self.getRobot().smartDashboard.putNumber("Loader Closed", loaderState)
def testLoader(self):
self.setLoader(self.State.kOpen)
print("Starting testLoader")
self.toggleLoader()
assert (self.loaderSolenoid.get() == DoubleSolenoid.Value.kForward)
wpilib.Timer.delay(5)
self.toggleLoader()
assert (self.loaderSolenoid.get() == DoubleSolenoid.Value.kReverse)
wpilib.Timer.delay(5)
print("Finish testLoader")
self.toggleLoader()
assert (self.loaderSolenoid.get() == DoubleSolenoid.Value.kForward)
wpilib.Timer.delay(2)
print("Finished testLoader now")
def setLoader(self, state):
#kReverse state claw is closed
#kForward state claw is open
if state == self.State.kOpen:
self.loaderSolenoid.set(DoubleSolenoid.Value.kForward)
elif state == self.State.kClose:
self.loaderSolenoid.set(DoubleSolenoid.Value.kReverse)
else:
raise ValueError("Invalid Loader State %s" % value)
self.currentState = state
class State(enum.IntEnum):
#state of the loader accuator
kOpen = 0
kClose = 1
def __init__(self):
super().__init__("Payload")
# TODO Two limit switches on the leader
self.prefs = Preferences.getInstance()
self.elbowleader = TalonSRX(CAN_ELBOW_LEADER)
set_motor(self.elbowleader, TalonSRX.NeutralMode.Brake, False)
self.elbowleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbowleader.selectProfileSlot(0, 0)
self.elbowleader.setSensorPhase(True)
self.elbowfollower = VictorSPX(CAN_ELBOW_FOLLOWER)
set_motor(self.elbowfollower, VictorSPX.NeutralMode.Brake, False)
self.elbowfollower.follow(self.elbowleader)
self.baller = TalonSRX(CAN_BALL_INTAKE)
set_motor(self.baller, TalonSRX.NeutralMode.Brake, False)
self.elbow_zero = False
self.DS = DoubleSolenoid(2, 3)
self.set_values()
class PlatePiston(Subsystem):
def __init__(self):
super().__init__("Plate Piston")
self.robot = team3200.getRobot()
self.map = self.robot.map.pneumaticsMap
self.platePiston = DoubleSolenoid(self.map.pcmCan,
self.map.forwardChannel,
self.map.reverseChannel)
def Activate(self):
'''Extends the piston if it is destended and vice versa.'''
print("Piston Activated")
if self.platePiston.get() == DoubleSolenoid.Value.kReverse:
self.platePiston.set(DoubleSolenoid.Value.kForward)
print("Piston Forwards")
#wpilib.Timer.delay(1)
elif self.platePiston.get() == DoubleSolenoid.Value.kForward:
self.platePiston.set(DoubleSolenoid.Value.kReverse)
print("Piston Backwards")
else:
self.platePiston.set(DoubleSolenoid.Value.kForward)
def __init__(self):
self.timer = Timer()
self.driver_station = DriverStation(controller=XboxController(0))
# self.gyroscope = AHRS.create_i2c()
# self.left_encoder = Encoder()
# self.right_encoder = Encoder()
self.drivetrain = TankDrivetrain(timer=self.timer, left_motors=[TalonSRX(10), TalonSRX(6)],
right_motors=[TalonSRX(12), TalonSRX(18)])
self.pneumatics = Pneumatics(compressor=Compressor(0), start_compressor=True, timer=self.timer)
self.beak = BeakMechanism(beak_solenoid=DoubleSolenoid(0, 4, 5), diag_solenoid=DoubleSolenoid(0, 0, 1),
driver_station=self.driver_station, timer=self.timer, cooldown=0.5)
# self.odometry = EncoderOdometry(left_encoder=self.left_encoder, right_encoder=self.right_encoder, gyro=self.gyroscope)
'''
class RearPuncher(Subsystem):
"""Puncher subsystem. This is a pneumatic puncher, implemented with
a double solenoid to fire the puncher."""
def __init__(self):
"""Assign and save the double solenoid assignment for the puncher."""
super().__init__()
self.rearpuncher = DoubleSolenoid(0, 0, 1)
def open(self):
"""Open (retract) the puncher."""
self.rearpuncher.set(DoubleSolenoid.Value.kReverse)
def close(self):
"""Close (extend) the puncher."""
self.rearpuncher.set(DoubleSolenoid.Value.kForward)
def stop(self):
"""Turn off the solenoid."""
self.rearpuncher.set(DoubleSolenoid.Value.kOff)
class Intake(Subsystem):
def __init__(self):
super().__init__("Intake")
self.talon_left = Talon(0)
self.talon_right = Talon(1)
self.solenoid_lift = DoubleSolenoid(0, 1)
self.solenoid_grab = DoubleSolenoid(2, 3)
self.set_grab_state(GrabState.IN)
self.set_arm_state(ArmState.UP)
self.power = .75
self.in_sensor = AnalogInput(0)
dashboard2.add_graph("Ultrasonic", self.get_reported_distance)
self.pdp = PowerDistributionPanel()
self.l_channel = 0
self.r_channel = 1
def is_stalled(self):
#l_current = self.pdp.getCurrent(self.l_channel)
#r_current = self.pdp.getCurrent(self.r_channel)
#avg_current = (l_current + r_current) / 2
#voltage = abs(self.talon_right.get() * self.pdp.getVoltage())
#bag_resistance = 12/53
#stall_current = voltage / bag_resistance
return False # voltage > 0.05 * 12 and abs(avg_current - stall_current) / stall_current < 0.05
def get_reported_distance(self):
return 20
def run_intake(self, power):
"""
positive power runs motors in
negative power runs motors out
:param power: The power to run the intake motors at
:return:
"""
self.talon_left.set(power)
self.talon_right.set(-power)
def intake(self):
self.run_intake(0.3)
def eject(self):
self.run_intake(-self.power)
def get_arm_state(self):
return ArmState.DOWN if self.solenoid_lift.get(
) == DoubleSolenoid.Value.kReverse else ArmState.UP
def set_arm_state(self, state):
if state == ArmState.DOWN:
self.solenoid_lift.set(DoubleSolenoid.Value.kReverse)
else:
self.solenoid_lift.set(DoubleSolenoid.Value.kForward)
def get_grab_state(self):
return GrabState.OUT if self.solenoid_grab.get(
) == DoubleSolenoid.Value.kForward else GrabState.IN
def set_grab_state(self, state):
if state == GrabState.OUT:
self.solenoid_grab.set(DoubleSolenoid.Value.kForward)
else:
self.solenoid_grab.set(DoubleSolenoid.Value.kReverse)
def has_acquired_cube(self):
if hal.isSimulation():
return False
return self.is_stalled()
def __init__(self):
super().__init__()
self.front_lift = DoubleSolenoid(0, 6, 7)
self.rear_lift = wpilib.DoubleSolenoid(0, 2, 3)
def __init__(self, can_id=0, channel_1=1, channel_2=2):
super().__init__()
self.solenoid = DoubleSolenoid(can_id, channel_1, channel_2)
class DriverComponent:
CONV_FACTOR = 0.0524 * 0.846
LINEAR_SAMPLE_RATE = 28
ANGULAR_SAMPLE_RATE = 2
def __init__(self):
left_front = WPI_TalonSRX(6)
left_rear = WPI_TalonSRX(1)
right_front = WPI_TalonSRX(4)
right_rear = WPI_TalonSRX(7)
left = SpeedControllerGroup(left_front, left_rear)
right = SpeedControllerGroup(right_front, right_rear)
self.left_encoder_motor = left_rear
self.right_encoder_motor = right_front
self.gear_solenoid = DoubleSolenoid(0, 1)
self.driver_gyro = ADXRS450_Gyro()
self.drive_train = DifferentialDrive(left, right)
# setup encoders
self.left_encoder_motor.setSensorPhase(True)
self.drive_train.setDeadband(0.1)
self.moving_linear = [0] * DriverComponent.LINEAR_SAMPLE_RATE
self.moving_angular = [0] * DriverComponent.ANGULAR_SAMPLE_RATE
def set_curve_raw(self, linear, angular):
if -0.1 < linear < 0.1:
self.drive_train.curvatureDrive(linear, angular, True)
else:
self.drive_train.curvatureDrive(linear, angular, False)
def set_curve(self, linear, angular):
self.moving_linear.append(linear)
self.moving_angular.append(angular)
if len(self.moving_linear) > DriverComponent.LINEAR_SAMPLE_RATE:
self.moving_linear.pop(0)
if len(self.moving_angular) > DriverComponent.ANGULAR_SAMPLE_RATE:
self.moving_angular.pop(0)
l_speed = sum([
x / DriverComponent.LINEAR_SAMPLE_RATE for x in self.moving_linear
])
a_speed = sum([
x / DriverComponent.ANGULAR_SAMPLE_RATE
for x in self.moving_angular
])
l_speed = math.sin(l_speed * math.pi / 2)
if -0.1 < l_speed < 0.1:
self.drive_train.curvatureDrive(l_speed, a_speed, True)
else:
self.drive_train.curvatureDrive(l_speed, a_speed, False)
def reset_drive_sensors(self):
self.driver_gyro.reset()
self.left_encoder_motor.setSelectedSensorPosition(0, 0, 0)
self.right_encoder_motor.setSelectedSensorPosition(0, 0, 0)
@property
def current_distance(self):
return DriverComponent.CONV_FACTOR * self.left_encoder_motor.getSelectedSensorPosition(
0)
def current_gear(self):
if self.gear_solenoid.get() is DoubleSolenoid.Value.kForward:
return GearMode.HIGH
if self.gear_solenoid.get() is DoubleSolenoid.Value.kReverse:
return GearMode.LOW
if self.gear_solenoid.get() is DoubleSolenoid.Value.kOff:
return GearMode.OFF
def toggle_gear(self):
if self.current_gear() is GearMode.LOW:
self.set_high_gear()
if self.current_gear() is GearMode.HIGH:
self.set_low_gear()
def set_low_gear(self):
print("shift low")
self.gear_solenoid.set(DoubleSolenoid.Value.kReverse)
def set_high_gear(self):
print("shift high")
self.gear_solenoid.set(DoubleSolenoid.Value.kForward)