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
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)
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()
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)