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)
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)
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]
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 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)
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 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)
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
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)
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 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)
class Payload(Subsystem): """ For both Hatches and Balls """ 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() def initDefaultCommand(self): self.setDefaultCommand(BallZ()) def set_wheels_speed(self, speed): self.baller.set(ControlMode.PercentOutput, speed) #print(speed) def hatch_punch_out(self): subsystems.SERIAL.fire_event('Punch It') self.DS.set(DoubleSolenoid.Value.kForward) def hatch_punch_in(self): self.DS.set(DoubleSolenoid.Value.kReverse) def set_position(self, pos): if self.elbow_zero: self.elbowleader.set(mode=ControlMode.MotionMagic, demand0=pos) else: print("Elbow Not Set") def set_speed(self, speed): self.elbowleader.set(mode=ControlMode.PercentOutput, demand0=speed) def set_values(self): self.elbowleader.config_kP(0, 0.8, 0) self.elbowleader.config_kI(0, 0.0, 0) self.elbowleader.config_kD(0, 0.0, 0) self.elbowleader.configMotionCruiseVelocity(int(200), 0) self.elbowleader.configMotionAcceleration(int(200), 0) def get_position(self): return self.elbowleader.getSelectedSensorPosition(0) def check_for_zero(self): if not self.elbow_zero: if self.elbowleader.isRevLimitSwitchClosed(): self.elbowleader.setSelectedSensorPosition(0, 0, 0) self.elbow_zero = True def publish_data(self): #print(self.elbowleader.getSelectedSensorPosition(0)) SmartDashboard.putNumber("elbowPosition", self.elbowleader.getSelectedSensorPosition(0))