def __init__(self, robot):
super().__init__("shooter")
self.robot = robot
self.counter = 0 # used for updating the log
self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional
# motor controllers
self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.spark_hood = Talon(5)
self.spark_feed = Talon(7)
# encoders and PID controllers
self.hood_encoder = Encoder(
4,
5) # generic encoder - we'll have to install one on the 775 motor
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.hood_controller = wpilib.controller.PIDController(Kp=0.005,
Ki=0,
Kd=0.0)
self.hood_setpoint = 5
self.hood_controller.setSetpoint(self.hood_setpoint)
self.flywheel_encoder = self.sparkmax_flywheel.getEncoder(
) # built-in to the sparkmax/neo
self.flywheel_controller = self.sparkmax_flywheel.getPIDController(
) # built-in PID controller in the sparkmax
# limit switches, use is TBD
self.limit_low = DigitalInput(6)
self.limit_high = DigitalInput(7)
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def __init__(self):
self.lower_shooter = Talon()
self.upper_shooter = Talon()
self.intake = Relay(0, Relay.Direction.kReverse)
self.launcher = DoubleSolenoid(,)
self.lifter = DoubleSolenoid(,)
self.should_lift = False
def __init__(self):
super().__init__()
self._l_motor = Talon(constants.motor_intake_l)
self._r_motor = Talon(constants.motor_intake_r)
self._intake_piston = Solenoid(constants.solenoid_intake)
self._photosensor = DigitalInput(constants.far_photosensor)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
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):
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 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__()
self._l_motor = Talon(hardware.intake_left)
self._r_motor = Talon(hardware.intake_right)
self._intake_piston = Solenoid(hardware.intake_solenoid)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
self._left_intake_amp = CircularBuffer(25)
self._right_intake_amp = CircularBuffer(25)
self._pulse_timer = Timer()
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 __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 __init__(self):
self.stateTimer = wpilib.Timer()
self.stateTimer.start()
self.ballRakeExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_EXTEND_SOLENOID)
self.ballRakeRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_RETRACT_SOLENOID)
self.ballHatchExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_EXTEND_SOLENOID)
self.ballHatchRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_RETRACT_SOLENOID)
self.ballTickler = Spark(robotmap.BALL_TICKLER_ID)
self.bottomIntake = Spark(robotmap.BOTTOM_INTAKE_ID)
self.rakeMotor = Talon(robotmap.RAKE_ID)
self.ballRakeExtend.set(False)
self.ballRakeRetract.set(True)
self.ballHatchExtend.set(False)
self.ballHatchRetract.set(True)
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 ShooterComponent():
def __init__(self):
self.lower_shooter = Talon()
self.upper_shooter = Talon()
self.intake = Relay(0, Relay.Direction.kReverse)
self.launcher = DoubleSolenoid(,)
self.lifter = DoubleSolenoid(,)
self.should_lift = False
def enable_intake(self):
self.intake.set(Relay.Value.kOn)
def disable_intake(self):
self.intake.set(Relay.Value.kOff)
def engage_launcher(self):
self.lifter.set(DoubleSolenoid.Value.kForward)
def retract_launcher(self):
self.lifter.set(DoubleSolenoid.Value.kReverse)
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.kForward)
def lower_lifter(self):
self.lifter.set(DoubleSolenoid.Value.kReverse)
def shoot(self, speed: float):
self.lower_shooter.set(speed)
self.upper_shooter.set(speed)
class Intake(Component):
def __init__(self):
super().__init__()
self._l_motor = Talon(constants.motor_intake_l)
self._r_motor = Talon(constants.motor_intake_r)
self._intake_piston = Solenoid(constants.solenoid_intake)
self._photosensor = DigitalInput(constants.far_photosensor)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
def update(self):
self._l_motor.set(self._left_pwm)
self._r_motor.set(self._right_pwm)
self._intake_piston.set(not self._open)
def spin(self, power, same_direction=False):
self._left_pwm = power
self._right_pwm = power * (1 if same_direction else -1)
def intake_tote(self):
power = .3 if not self._photosensor.get() else .75
self.spin(power)
def intake_bin(self):
power = .3 if not self._photosensor.get() else .65
self.spin(power)
def open(self):
self._open = True
def close(self):
self._open = False
def stop(self):
"""Disables EVERYTHING. Only use in case of critical failure"""
self._l_motor.set(0)
self._r_motor.set(0)
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 Shooter(Subsystem):
# ----------------- INITIALIZATION -----------------------
def __init__(self, robot):
super().__init__("shooter")
self.robot = robot
self.counter = 0 # used for updating the log
self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional
# motor controllers
self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.spark_hood = Talon(5)
self.spark_feed = Talon(7)
# encoders and PID controllers
self.hood_encoder = Encoder(
4,
5) # generic encoder - we'll have to install one on the 775 motor
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.hood_controller = wpilib.controller.PIDController(Kp=0.005,
Ki=0,
Kd=0.0)
self.hood_setpoint = 5
self.hood_controller.setSetpoint(self.hood_setpoint)
self.flywheel_encoder = self.sparkmax_flywheel.getEncoder(
) # built-in to the sparkmax/neo
self.flywheel_controller = self.sparkmax_flywheel.getPIDController(
) # built-in PID controller in the sparkmax
# limit switches, use is TBD
self.limit_low = DigitalInput(6)
self.limit_high = DigitalInput(7)
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
#self.setDefaultCommand(ShooterHoodAxis(self.robot))
def set_flywheel(self, velocity):
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0, self.feed_forward)
self.flywheel_controller.setReference(velocity,
rev.ControlType.kSmartVelocity,
0)
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0)
def stop_flywheel(self):
self.flywheel_controller.setReference(0, rev.ControlType.kVoltage)
def set_feed_motor(self, speed):
self.spark_feed.set(speed)
def stop_feed_motor(self):
self.spark_feed.set(0)
def set_hood_motor(self, power):
power_limit = 0.2
if power > power_limit:
new_power = power_limit
elif power < -power_limit:
new_power = -power_limit
else:
new_power = power
self.spark_hood.set(new_power)
def change_elevation(
self, power
): # open loop approach - note they were wired to be false when contacted
if power > 0 and self.limit_high.get():
self.set_hood_motor(power)
elif power < 0 and self.limit_low.get():
self.set_hood_motor(power)
else:
self.set_hood_motor(0)
def set_hood_setpoint(self, setpoint):
self.hood_controller.setSetpoint(setpoint)
def get_angle(self):
elevation_minimum = 30 # degrees
conversion_factor = 1 # encoder units to degrees
# return elevation_minimum + conversion_factor * self.hood_encoder.getDistance()
return self.hood_encoder.getDistance()
def periodic(self) -> None:
"""Perform necessary periodic updates"""
self.counter += 1
if self.counter % 5 == 0:
# pass
# ten per second updates
SmartDashboard.putNumber('elevation',
self.hood_encoder.getDistance())
SmartDashboard.putNumber('rpm',
self.flywheel_encoder.getVelocity())
watch_axis = False
if watch_axis:
self.hood_scale = 0.2
self.hood_offset = 0.0
power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) -
0.5) + self.hood_offset
self.robot.shooter.change_elevation(power)
maintain_elevation = True
if maintain_elevation:
self.error = self.get_angle() - self.hood_setpoint
pid_out = self.hood_controller.calculate(self.error)
output = 0.03 + pid_out
SmartDashboard.putNumber('El PID', pid_out)
self.change_elevation(output)
if self.counter % 50 == 0:
SmartDashboard.putBoolean("hood_low", self.limit_low.get())
SmartDashboard.putBoolean("hood_high", self.limit_high.get())
class Shooter:
left_fly = CANTalon
right_fly = CANTalon
intake_main = CANTalon
intake_mecanum = Talon
ball_limit = DigitalInput
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def warm_up(self):
self.set_rpm(2500) # Warm up flywheels to get ready to shoot
def low_goal(self):
self.set_rpm(500)
def set_rpm(self, rpm_l_set, rpm_r_set=None):
if rpm_r_set is None:
rpm_r_set = rpm_l_set
self.left_fly.set(rpm_l_set)
self.right_fly.set(rpm_r_set)
def get_rpms(self):
return self.left_fly.get(), self.right_fly.get()
def set_fly_off(self):
self.set_rpm(0)
def set_intake(self, power):
self.intake_mecanum.set(-power)
self.intake_main.set(power)
def get_intake(self):
return self.intake_main.get()
def set_intake_off(self):
self.set_intake(0)
def get_ball_limit(self):
return not bool(self.ball_limit.get())
def execute(self):
if int(self.left_fly.getOutputCurrent()) > 20 \
or int(self.left_fly.getOutputCurrent()) > 20:
self.set_rpm(0, 0)
class Intake(Component):
def __init__(self):
super().__init__()
self._l_motor = Talon(hardware.intake_left)
self._r_motor = Talon(hardware.intake_right)
self._intake_piston = Solenoid(hardware.intake_solenoid)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
self._left_intake_amp = CircularBuffer(25)
self._right_intake_amp = CircularBuffer(25)
self._pulse_timer = Timer()
def update(self):
self._l_motor.set(self._left_pwm)
self._r_motor.set(-self._right_pwm)
self._intake_piston.set(not self._open)
self._left_intake_amp.append(hardware.left_intake_current())
self._right_intake_amp.append(hardware.right_intake_current())
def intake_tote(self):
if hardware.game_piece_in_intake(): # anti-bounce & slowdown
power = .8
else:
power = .3 if not hardware.back_photosensor.get() else 1
self.set(power)
if self.intake_jammed() and not self._pulse_timer.running:
self._pulse_timer.start()
if self._pulse_timer.running:
self.set(-self._pulse_timer.get() / 2)
if self._pulse_timer.get() > .05:
self._pulse_timer.stop()
self._pulse_timer.reset()
def intake_bin(self):
power = .3 if not hardware.back_photosensor.get() else .65
self.set(power, power)
def pause(self):
self.set(0)
def open(self):
self._open = True
def close(self):
self._open = False
def stop(self):
self._l_motor.set(0)
self._r_motor.set(0)
def set(self, l, r=None):
""" Spins the intakes at a given power. Pass either a power or left and right power. """
self._left_pwm = l
if r is None:
self._right_pwm = l
else:
self._right_pwm = r
def intake_jammed(self):
if not hardware.back_photosensor.get():
return False
return self._left_intake_amp.average > AMP_THRESHOLD or self._right_intake_amp.average > AMP_THRESHOLD
def update_nt(self):
log.info("left current: %s" % self._left_intake_amp.average)
log.info("right current: %s" % self._right_intake_amp.average)
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 BallChute():
def __init__(self):
self.stateTimer = wpilib.Timer()
self.stateTimer.start()
self.ballRakeExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_EXTEND_SOLENOID)
self.ballRakeRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_RAKE_RETRACT_SOLENOID)
self.ballHatchExtend = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_EXTEND_SOLENOID)
self.ballHatchRetract = Solenoid(robotmap.PCM2_ID, robotmap.BALL_HATCH_RETRACT_SOLENOID)
self.ballTickler = Spark(robotmap.BALL_TICKLER_ID)
self.bottomIntake = Spark(robotmap.BOTTOM_INTAKE_ID)
self.rakeMotor = Talon(robotmap.RAKE_ID)
self.ballRakeExtend.set(False)
self.ballRakeRetract.set(True)
self.ballHatchExtend.set(False)
self.ballHatchRetract.set(True)
#-value so motors run that way we want them to
def RakeMotorStart(self, value):
self.rakeMotor.set(-value)
def RakeMotorStop(self):
self.rakeMotor.stopMotor()
def BottomMotorStart(self, value):
self.bottomIntake.set(-value)
def BottomMotorStop(self):
self.bottomIntake.stopMotor()
def BallTicklerStart(self, value):
self.ballTickler.set(value)
def BallTicklerStop(self):
self.ballTickler.stopMotor()
#Solenoid functions ot make things easier
def DeployRake(self):
self.ballRakeExtend.set(True)
self.ballRakeRetract.set(False)
def StowRake(self):
self.ballRakeExtend.set(False)
self.ballRakeRetract.set(True)
def OpenHatch(self):
self.ballHatchExtend.set(True)
self.ballHatchRetract.set(False)
def CloseHatch(self):
self.ballHatchExtend.set(False)
self.ballHatchRetract.set(True)
def Iterate(self, copilot: XBox):
if copilot.Y():
if copilot.Start():
self.BottomMotorStart(1)
elif copilot.X():
self.BottomMotorStart(-.5)
else:
self.BottomMotorStart(.5)
else:
self.BottomMotorStop()
if copilot.A():
elif copilot.X():
self.RakeMotorStart(-.5)
else:
self.RakeMotorStart(.5)
else:
