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)