def robotInit(self):
''' function that is run at the beginning of the match '''
self.bottomShooterEncoder = WPI_TalonSRX(7)
self.topShooter = WPI_TalonSRX(6)
self.bottomShooter = WPI_TalonSRX(4)
self.topShooterEncoder = WPI_TalonSRX(5)
self.xbox = wpilib.Joystick(2) # controller for shooter
self.topShooters = wpilib.SpeedControllerGroup(self.topShooter,
self.topShooterEncoder)
self.bottomShooters = wpilib.SpeedControllerGroup(
self.bottomShooter, self.bottomShooterEncoder)
self.dash = NetworkTables.getTable("limelight")
self.dashboard = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.99.91.2')
self.topButtonStatus = Toggle(self.xbox, 1)
self.bottomButtonStatus = Toggle(self.xbox, 4)
# PID settings
kP = 0.0
kI = 0.0
kD = 0.0
def robotInit(self):
""" Functions """
self.dashboard = Dashboard()
self.drive = Drive()
self.indexer = Indexer()
self.intake = Intake()
self.lift = Lift()
self.semicircle = Semicircle()
self.shooter = Shooter()
self.vision = Vision()
""" Joystick """
# setting joysticks and xbox controllers
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
""" Button Status and Toggles """
# button for switching between arcade and tank drive
self.driveButtonStatus = Toggle(self.rightJoystick, 2)
# button for gear shifting
self.gearButtonStatus = Toggle(self.rightJoystick, 1)
# button for lift actuation
self.liftButtonStatus = Toggle(self.xbox, 5)
# button to run intake, indexer, and semicircle
self.intakeBall = self.xbox.getRawAxis(1)
self.dpadForward = False
self.dpadBackwards = False
# button for autoaim
self.turnButtonStatus = self.xbox.getRawButton(6)
""" Sensors """
# color sensor
i2cPort = wpilib.I2C.Port.kOnboard
self.colorSensor = ColorSensorV3(i2cPort)
self.colorSensitivity = 170 # boundary between not seeing an object and seeing an object
""" Limit Switch """
self.limitSwitch = wpilib.DigitalInput(0)
self.ballsInPossession = 0
self.limitSwitchTriggered = False
""" Pneumatics """
# pneumatic compressor
self.compressor = wpilib.Compressor(0)
self.compressor.setClosedLoopControl(True)
self.compressor.start()
""" Shooter """
self.setpointReached = False
self.shooterRun = False
""" NavX """
# self.drive.navx.reset()
""" Timer """
self.timer = wpilib.Timer()
self.dashboard.limelightLed(False)
def test_toggle():
joystick = FakeJoystick()
toggleButton = Toggle(joystick, 0)
toggleButton2 = Toggle(joystick, 1)
assert toggleButton.off
joystick.press(0)
assert toggleButton.on
assert toggleButton2.off
joystick.release(0)
assert toggleButton.on
joystick.press(0)
assert toggleButton.off
joystick.release(0)
assert toggleButton.off
joystick.press(1)
assert toggleButton.off
assert toggleButton2.on
def test_toggle_debounce():
delay = PreciseDelay(2.1)
joystick = FakeJoystick()
toggleButton = Toggle(joystick, 1, 2)
assert toggleButton.off
joystick.press()
assert toggleButton.on
joystick.release()
joystick.press()
joystick.release()
assert toggleButton.on
delay.wait()
assert toggleButton.on
joystick.press()
assert toggleButton.off
def robotInit(self):
self.xbox = wpilib.XboxController(0)
self.lift_lock = Toggle(self.xbox, 4, 0.5)
self.left_drive_motor_group = wpilib.SpeedControllerGroup(
wpilib.Talon(0), wpilib.Talon(1))
self.right_drive_motor_group = wpilib.SpeedControllerGroup(
wpilib.Talon(2), wpilib.Talon(3))
self.drive = DifferentialDrive(self.left_drive_motor_group,
self.right_drive_motor_group)
self.drive_rev = False
self.speedRatio = 0.5
self.lift_motor = wpilib.SpeedControllerGroup(wpilib.Spark(4),
wpilib.Spark(5))
self.lift_motor_speed = 0.0
# ball grab motors, need to spin in opposite directions
self.front_motor_1 = wpilib.Spark(6)
self.front_motor_2 = wpilib.Spark(7)
self.front_motor_2.setInverted(True)
self.front_motor = wpilib.SpeedControllerGroup(self.front_motor_1,
self.front_motor_2)
self.hatch_solenoid = wpilib.DoubleSolenoid(
0, 1) # pneumatic channels 0 & 1
self.encoder = wpilib.Encoder(aChannel=0, bChannel=1) # DIO 0 & 1
wpilib.CameraServer.launch(
'vision.py:main') # setup cameras: usb1 & usb2
self.xbox_axis = {} # init xbox axis dict
self.debug = False # enable to print debug info
self.reset() # i.e. ensure defaults are set
self.loops = 0 # counter for program loops
self.timer = wpilib.Timer() # init timer
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
class Robot(magicbot.MagicRobot):
# Order of components determines order that execute methods are run
# State Machines
panel_spinner: PanelSpinner
auto_launcher: AutoShoot
# Other components
align: Align
odometry: Odometry
follower: Follower
intake: Intake
control_panel: ControlPanel
launcher: Launcher
drive: Drive
flipped = tunable(False)
ntSolenoid_state = ntproperty("/robot/ntSolenoid_state", 0)
ds_velocity_setpoint = ntproperty('/components/launcher/DS_VEL_SETPOINT',
2100)
limelight_stream_mode = ntproperty('/limelight/stream', 2)
WHEEL_DIAMETER = 0.1524 # Units: Meters
ENCODER_PULSES_PER_REV = 2048 # Ignore quadrature decoding (4x, 8192)
LAUNCHER_GEARING = 1 # No gearing since encoder is on shaft
GEARING = 7.56 # 7.56:1 gear ratio
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
def autonomous(self):
self.right_motors.setInverted(True)
super().autonomous()
self.limelight.changePipeline(0)
def disabledInit(self):
self.navx.reset()
self.limelight_stream_mode = 0
def disabledPeriodic(self):
self.limelight.averagePose()
def teleopInit(self):
self.right_motors.setInverted(False)
self.drive.squared_inputs = True
self.drive.speed_constant = 1.05
self.limelight.changePipeline(0)
self.drive.rotational_constant = 0.5
self.inverse = 1
def teleopPeriodic(self):
# if self.btn_invert_y_axis.get():
# self.flipped = True
# self.inverse *= -1
# else:
# self.flipped = False
# self.logger.info(self.ultrasonic.getRangeInches())
if self.btn_invert_y_axis.get():
self.inverse = 1
else:
self.inverse = -1
if self.btn_rotation_sensitivity.get():
self.drive.rotational_constant = 0.1
self.drive.move(self.inverse * self.joystick_left.getY(),
self.joystick_right.getX())
# Align (Overrides self.drive.move() because it's placed after)
if self.btn_align.get() and self.limelight.targetExists():
self.drive.set_target(self.limelight.getYaw(), relative=True)
if self.btn_align.get():
self.limelight.TurnLightOn(True)
self.drive.align()
else:
self.limelight.TurnLightOn(False)
self.drive.set_target(None)
if self.btn_slow_movement.get():
# 10% of original values
self.drive.rotational_constant = 0.54
self.drive.speed_constant = 0.5
# Control Panel Spinner
self.control_panel.set_solenoid(self.btn_cp_extend.get())
if self.btn_cp_extend.get():
self.ntSolenoid_state = True
else:
self.ntSolenoid_state = False
if self.btn_scissor_extend.get():
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
else:
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
# Color Sensor
if self.btn_color_sensor.get():
self.panel_spinner.spin_to()
if self.btn_cp_motor.get():
self.control_panel.spin(0.5)
# Launcher
if self.btn_launcher_motor.get():
self.launcher.setVelocity(4630)
elif self.btn_launcher_motor_close.get():
self.launcher.setVelocity(3900)
elif self.btn_launcher_motor_dynamic.get():
# self.limelight.TurnLightOn(True)
self.launcher.setVelocity(self.ds_velocity_setpoint)
# if self.limelight.targetExists():
# self.launcher.setVelocityFromDistance(self.limelight.pitch_angle, 4670)
elif self.btn_launcher_idle.get():
self.launcher.setPercentOutput(1)
if self.btn_launcher_solenoid.get():
self.auto_launcher.fire_when_ready()
if self.btn_cp_stop.get():
self.panel_spinner.done()
# Intake
if self.btn_intake_out.get():
self.intake.spin_inside(1)
elif self.btn_intake_in.get():
self.intake.spin(-1, 0.4)
elif self.btn_intake_bottom_out.get():
self.intake.spin_lower(-0.4)
if self.btn_intake_solenoid.get():
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
# Winch
if self.btn_winch.get():
self.winch_motors.set(0.65)
else:
self.winch_motors.set(
0
) # Must use set(0) when not pressed because there is no component
# Hook
if self.joystick_alt.getPOV(0) == 90:
self.hook_motor.set(0.5)
elif self.joystick_alt.getPOV(0) == 270:
self.hook_motor.set(-0.5)
else:
self.hook_motor.set(0)
if self.joystick_alt.getPOV(0) == 0:
self.launcher.fire()
def robotInit(self):
''' Initialization of robot objects. '''
''' Talon SRX Initialization '''
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
''' Motor Groups '''
# drive train motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
''' Victor SPX Initialization '''
# lift motors
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motors
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor
self.cargo = WPI_VictorSPX(5)
''' Encoders '''
# drive train encoders
self.rightEncoder = self.frontRightMotor
self.leftEncoder = self.frontLeftMotor
# lift encoder
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder
self.liftArmEncoder = wpilib.Encoder(5, 6, True)
''' Sensors '''
# Hall Effect Sensor
self.minHall = wpilib.DigitalInput(7)
self.maxHall = wpilib.DigitalInput(4)
self.limitSwitch = wpilib.DigitalInput(3)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' Controller Initialization and Mapping '''
# joystick - 0, 1 | controller - 2
self.joystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
''' Pneumatic Button Status '''
self.clawButtonStatus = Toggle(self.xbox, 2)
self.gearButtonStatus = Toggle(self.joystick, 1)
self.ejectorPinButtonStatus = Toggle(self.xbox, 1)
self.compressorButtonStatus = Toggle(self.xbox, 9)
self.liftHeightButtonStatus = Toggle(self.xbox, 3)
''' Pneumatic Initialization '''
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
''' Smart Dashboard '''
# connection for logging & Smart Dashboard
logging.basicConfig(level=logging.DEBUG)
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
# Smart Dashboard classes
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
''' Timer '''
self.timer = wpilib.Timer()
''' Camera '''
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
''' PID settings for lift '''
self.kP = 0.03
self.kI = 0.0
self.kD = 0.0
self.kF = 0.1
self.PIDLiftcontroller = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.liftEncoder,
output=self)
self.PIDLiftcontroller.setInputRange(0, 400)
self.PIDLiftcontroller.setOutputRange(-0.5, 0.5)
self.PIDLiftcontroller.setAbsoluteTolerance(1.0)
self.PIDLiftcontroller.setContinuous(True)
self.encoderRate = 0
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
''' Initialization of robot objects. '''
''' Talon SRX Initialization '''
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
''' Motor Groups '''
# drive train motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
''' Victor SPX Initialization '''
# lift motors
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motors
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor
self.cargo = WPI_VictorSPX(5)
''' Encoders '''
# drive train encoders
self.rightEncoder = self.frontRightMotor
self.leftEncoder = self.frontLeftMotor
# lift encoder
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder
self.liftArmEncoder = wpilib.Encoder(5, 6, True)
''' Sensors '''
# Hall Effect Sensor
self.minHall = wpilib.DigitalInput(7)
self.maxHall = wpilib.DigitalInput(4)
self.limitSwitch = wpilib.DigitalInput(3)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' Controller Initialization and Mapping '''
# joystick - 0, 1 | controller - 2
self.joystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
''' Pneumatic Button Status '''
self.clawButtonStatus = Toggle(self.xbox, 2)
self.gearButtonStatus = Toggle(self.joystick, 1)
self.ejectorPinButtonStatus = Toggle(self.xbox, 1)
self.compressorButtonStatus = Toggle(self.xbox, 9)
self.liftHeightButtonStatus = Toggle(self.xbox, 3)
''' Pneumatic Initialization '''
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
''' Smart Dashboard '''
# connection for logging & Smart Dashboard
logging.basicConfig(level=logging.DEBUG)
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
# Smart Dashboard classes
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
''' Timer '''
self.timer = wpilib.Timer()
''' Camera '''
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
''' PID settings for lift '''
self.kP = 0.03
self.kI = 0.0
self.kD = 0.0
self.kF = 0.1
self.PIDLiftcontroller = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.liftEncoder,
output=self)
self.PIDLiftcontroller.setInputRange(0, 400)
self.PIDLiftcontroller.setOutputRange(-0.5, 0.5)
self.PIDLiftcontroller.setAbsoluteTolerance(1.0)
self.PIDLiftcontroller.setContinuous(True)
self.encoderRate = 0
def pidWrite(self, output):
self.encoderRate = output
def robotCode(self):
if self.liftHeightButtonStatus.on:
self.PIDLiftcontroller.setSetpoint(200)
self.liftToHeight = True
elif self.liftHeightButtonStatus.off:
self.PIDLiftcontroller.setSetpoint(0)
self.liftToHeight = False
def hatchOne():
if self.liftEncoder.getDistance() < 80: # Hatch 2
self.lift.set(0.3)
elif self.liftEncoder.getDistance() >= 80:
self.lift.set(0.07)
def hatchTwo():
if self.liftEncoder.getDistance() < 275: # Hatch 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 275:
self.lift.set(0.07)
def cargoOne():
if self.liftEncoder.getDistance() < 150: # Cargo 1
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 150:
self.lift.set(0.05)
def cargoTwo():
if self.liftEncoder.getDistance() < 320: # Cargo 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 320:
self.lift.set(0.05)
def cargoShip():
if self.liftEncoder.getDistance() < 280: # Cargo ship
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 280:
self.lift.set(0.07)
# ''' Button Box Level Mapping '''
# if self.buttonStatusOne.on:
# # hatchOne()
# cargoOne()
# elif self.buttonStatusTwo.on: # comment out for hatch
# cargoTwo()
# elif self.buttonStatusThree.on:
# # hatchTwo()
# cargoShip()
if self.minHall.get() is False:
self.liftEncoder.reset()
if self.limitSwitch.get() is False:
self.liftArmEncoder.reset()
''' Smart Dashboard '''
# compressor state
if self.Compressor.enabled() is True:
self.sd.putString("Compressor Status: ", "Enabled")
elif self.Compressor.enabled() is False:
self.sd.putString("Compressor Status: ", "Disabled")
''' Pneumatics Dashboard States '''
# gear state
if self.DoubleSolenoidOne.get() == 1:
self.sd.putString("Gear Shift: ", "HIGH SPEED!!!")
elif self.DoubleSolenoidOne.get() == 2:
self.sd.putString("Gear Shift: ", "Low")
# ejector state
if self.DoubleSolenoidThree.get() == 2:
self.sd.putString("Ejector Pins: ", "Ejected")
elif self.DoubleSolenoidThree.get() == 1:
self.sd.putString("Ejector Pins: ", "Retracted")
# claw state
if self.DoubleSolenoidTwo.get() == 2:
self.sd.putString("Claw: ", "Open")
elif self.DoubleSolenoidTwo.get() == 1:
self.sd.putString("Claw: ", "Closed")
''' Ultrasonic Range Detection '''
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
if 0.142 <= self.ultraValue <= 0.146:
self.sd.putString("PLAYER STATION RANGE: ", "YES!!!!")
else:
self.sd.putString("PLAYER STATION RANGE: ", "NO!")
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
if 0.70 <= self.cargoUltraValue <= 1.56:
self.sd.putString("HATCH RANGE: ", "HATCH IN RANGE")
else:
self.sd.putString("HATCH RANGE: ", "NOT IN RANGE")
''' Pneumatics Toggles '''
# Compressor
if self.compressorButtonStatus.on:
self.Compressor.start()
elif self.compressorButtonStatus.off:
self.Compressor.stop()
# Claw Toggle
if self.clawButtonStatus.on:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward) # open claw
elif self.clawButtonStatus.off:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse) # close claw
# Ejector Pins Toggle
if self.ejectorPinButtonStatus.on:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward) # eject
elif self.ejectorPinButtonStatus.off:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse) # retract
# Gear Shift Toggle
if self.gearButtonStatus.on:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward) # shift right
elif self.gearButtonStatus.off:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse) # shift left
''' Victor SPX (Lift, Lift Arm, Cargo) '''
# lift control
if self.liftHeightButtonStatus.get() is False:
if self.xbox.getRawButton(5): # hold
self.lift.set(0.05)
elif self.xbox.getRawAxis(3): # up
self.lift.set(self.xbox.getRawAxis(3) * 0.85)
elif self.xbox.getRawAxis(2): # down
self.lift.set(-self.xbox.getRawAxis(2) * 0.45)
else:
self.lift.set(0)
# else:
# if self.liftToHeight is True:
# self.PIDLiftcontroller.enable()
# self.liftHeight = self.encoderRate
# self.lift.set(self.liftHeight)
# else:
# self.PIDLiftcontroller.disable()
# self.lift.set(0)
# # four-bar control
# if self.xbox.getRawButton(6): # hold
# self.liftArm.set(0.12)
# elif not self.xbox.getRawButton(6):
# self.liftArm.set(-self.xbox.getRawAxis(1) * 0.35)
# else:
# self.liftArm.set(0)
# cargo intake control
if self.xbox.getRawButton(7): # hold
self.cargo.set(0.12)
elif self.xbox.getRawAxis(5): # take in
self.cargo.set(self.xbox.getRawAxis(5) * 0.75)
# controller mapping for arcade steering
self.driveAxis = self.joystick.getRawAxis(1)
self.rotateAxis = self.joystick.getRawAxis(2)
# drives drive system using tank steering
if self.DoubleSolenoidOne.get() == 1: # if on high gear
self.divisor = 1.0 # 90% of high speed
self.turnDivisor = 0.8
elif self.DoubleSolenoidOne.get() == 2: # if on low gear
self.divisor = 0.85 # normal slow speed
self.turnDivisor = 0.75
else:
self.divisor = 1.0
if self.driveAxis != 0:
self.leftSign = self.driveAxis / fabs(self.driveAxis)
else:
self.leftSign = 0
if self.rotateAxis != 0:
self.rightSign = self.rotateAxis / fabs(self.rotateAxis)
else:
self.rightSign = 0
self.drive.arcadeDrive(-self.driveAxis * self.divisor,
self.rotateAxis * 0.75)
def autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# timer config
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
self.liftArmEncoder.reset()
def autonomousPeriodic(self):
self.sd.putBoolean("LIFT RESET ", self.minHall.get())
''' Called periodically during autonomous. '''
'''Test Methods'''
def encoder_test():
''' Drives robot set encoder distance away '''
self.rightPos = fabs(self.rightEncoder.getQuadraturePosition())
self.leftPos = fabs(self.leftEncoder.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
def Diagnostics():
''' Smart Dashboard Tests'''
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber("Right Encoder Speed: ",
abs(self.rightEncoder.getQuadratureVelocity()))
self.sd.putNumber("Left Encoder Speed: ",
abs(self.leftEncoder.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ", self.liftEncoder.getDistance())
def Pressure():
self.Compressor.start()
''' Test Execution '''
if self.DS.getGameSpecificMessage() == "pressure":
Pressure()
elif self.DS.getGameSpecificMessage() == "diagnostics":
Diagnostics()
self.robotCode()
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
self.robotCode()
class Manticore(wpilib.TimedRobot):
def robotInit(self):
""" Functions """
self.dashboard = Dashboard()
self.drive = Drive()
self.indexer = Indexer()
self.intake = Intake()
self.lift = Lift()
self.semicircle = Semicircle()
self.shooter = Shooter()
self.vision = Vision()
""" Joystick """
# setting joysticks and xbox controllers
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
""" Button Status and Toggles """
# button for switching between arcade and tank drive
self.driveButtonStatus = Toggle(self.rightJoystick, 2)
# button for gear shifting
self.gearButtonStatus = Toggle(self.rightJoystick, 1)
# button for lift actuation
self.liftButtonStatus = Toggle(self.xbox, 5)
# button to run intake, indexer, and semicircle
self.intakeBall = self.xbox.getRawAxis(1)
self.dpadForward = False
self.dpadBackwards = False
# button for autoaim
self.turnButtonStatus = self.xbox.getRawButton(6)
""" Sensors """
# color sensor
i2cPort = wpilib.I2C.Port.kOnboard
self.colorSensor = ColorSensorV3(i2cPort)
self.colorSensitivity = 170 # boundary between not seeing an object and seeing an object
""" Limit Switch """
self.limitSwitch = wpilib.DigitalInput(0)
self.ballsInPossession = 0
self.limitSwitchTriggered = False
""" Pneumatics """
# pneumatic compressor
self.compressor = wpilib.Compressor(0)
self.compressor.setClosedLoopControl(True)
self.compressor.start()
""" Shooter """
self.setpointReached = False
self.shooterRun = False
""" NavX """
# self.drive.navx.reset()
""" Timer """
self.timer = wpilib.Timer()
self.dashboard.limelightLed(False)
def autonomousInit(self):
self.drive.rearRightEncoder.setSelectedSensorPosition(0)
self.drive.rearLeftEncoder.setSelectedSensorPosition(0)
self.dashboard.limelightLed(True)
self.timer.reset()
self.timer.start()
# self.encoderReset1 = False
# self.encoderReset2 = False
# self.encoderReset3 = False
# self.turnAngle1 = False
# self.turnAngle2 = False
# self.turnAngle3 = False
# self.firstPeriodDone = False
def autonomousPeriodic(self):
if self.timer.get() <= 4.0:
""" Limelight """
self.dashboard.limelightLed(True)
self.tx = self.dashboard.limelight(
'tx') # getting horizontal angle to target
self.ty = self.dashboard.limelight(
'ty') # getting vertical angle to target
self.dashboard.limelightHorizontalAngle(self.tx)
self.distance = self.vision.getDistance(self.ty)
self.drive.turnToTarget(self.tx)
elif 4.0 < self.timer.get() <= 8.0:
""" Shooter """
# shooter rpm
if self.timer.get() < 4.5:
self.semicircle.run('Reverse')
self.shooterRPMTop = (abs(self.shooter.getShooterRPM('Top')))
self.shooterRPMBottom = (abs(self.shooter.getShooterRPM('Bottom')))
# sets shooter at a certain RPM if the trigger is being pressed
if self.distance < 170:
self.targetRPMTop = 3050 + (-19.3 * self.distance) + (
0.0571 * (self.distance * self.distance))
elif self.distance > 170:
self.targetRPMTop = -3600 + (46.1 * self.distance) + (
-0.101 * (self.distance * self.distance))
# sets shooter at a certain RPM if right trigger is being pressed
self.targetRPMBottom = self.targetRPMTop * 2 # introducing backspin
self.shooter.setSetpoint('Top', self.targetRPMTop)
self.shooter.setSetpoint('Bottom', self.targetRPMBottom)
self.setpointReached = False
self.shooterRun = True
self.dashboard.limelightLed(False)
self.shooter.execute('Top')
self.shooter.execute('Bottom')
self.ballsInPossession = 0
error = 100 # allowing 100 RPM error
if (self.targetRPMTop - error) <= self.shooterRPMTop <= (self.targetRPMTop + error) and \
(self.targetRPMBottom - error) <= self.shooterRPMBottom <= (self.targetRPMBottom + error) and \
self.setpointReached is False:
self.setpointReached = True
else:
self.setpointReached = False
if self.shooterRun is True and self.setpointReached is False:
# runs while shooter is activated but not yet at target RPM
self.intake.run('Forward')
self.indexer.run('Stop')
self.semicircle.run('Stop')
elif self.setpointReached is True:
# runs while shooter is activated and target RPM is reached
self.intake.run('Forward')
self.indexer.run('Forward')
self.semicircle.semicircleMotor.set(0.50)
elif 8.0 < self.timer.get() < 11:
self.drive.tankDrive(0.6, 0.6)
elif self.timer.get() > 10.0:
self.dashboard.limelightLed(False)
self.drive.tankDrive(0, 0)
# # timer
# self.timer.reset()
# self.timer.start()
#
# # limelight
# self.tx = self.dashboard.limelight('tx')
#
# # encoder
# self.driveTrainEncoder = (self.drive.rearLeftEncoder.getSelectedSensorPosition() + self.drive.rearRightEncoder.getSelectedSensorPosition()) / 2
#
# # shooter
# self.shooter.setSetpoint('Top', 2000)
# self.shooter.setSetpoint('Bottom', 2000)
#
# # runs between 0 and 5 seconds
# if self.timer.get() < 5:
# self.drive.turnToTarget(self.tx)
# self.shooter.execute('Top')
# self.shooter.execute('Bottom')
# if abs(self.tx) < 2:
# self.semicircle.run('Forward')
#
# # runs between 5 and 10
# elif 5 < self.timer.get() > 10:
# if self.firstPeriodDone == False:
# self.semicircle.run('Stop')
# self.firstPeriodDone = True
# self.shooter.setSetpoint('Top', 0)
# self.shooter.setSetpoint('Bottom', 0)
# if self.encoderReset1 == False:
# self.drive.rearRightEncoder.setSelectedSensorPosition(0)
# self.drive.rearLeftEncoder.setSelectedSensorPosition(0)
# self.encoderReset1 = True
# if self.turnAngle1 == False:
# self.drive.turnAngle(-150)
# self.turnAngle1 = True
# if self.encoderReset2 == False:
# self.drive.rearRightEncoder.setSelectedSensorPosition(0)
# self.drive.rearLeftEncoder.setSelectedSensorPosition(0)
# self.encoderReset2 = True
# self.drive.drive.tankDrive(0.5, 0.5)
# self.intake.run('Forward')
# self.indexer.run('Forward')
# self.semicircle.semicircleMotor.set(1)
# if self.driveTrainEncoder > 16384:
# self.drive.drive.stopMotor()
# else:
# pass
#
# # runs past 10 seconds
# if self.timer.hasPeriodPassed(10):
# if self.encoderReset3 == False:
# self.drive.rearRightEncoder.setSelectedSensorPosition(0)
# self.drive.rearLeftEncoder.setSelectedSensorPosition(0)
# self.encoderReset3 = True
# if self.turnAngle2 == False and self.encoderReset3 == True:
# self.drive.turnAngle(135)
# self.turnAngle2 = True
# if self.turnAngle3 == False and self.turnAngle2 == True:
# self.drive.turnAngle(self.tx)
# self.turnAngle3 = True
# if self.turnAngle3 == True:
# self.shooter.setSetpoint('Top', 2000)
# self.shooter.setSetpoint('Bottom', 4000)
# self.intake.run('Forward')
# self.indexer.run('Forward')
# self.semicircle.semicircleMotor.set(1)
def teleopInit(self):
self.drive.gearSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
self.drive.navx.reset()
self.drive.rearRightEncoder.setSelectedSensorPosition(0)
self.drive.rearLeftEncoder.setSelectedSensorPosition(0)
self.dashboard.limelightLed(False)
self.shooterLaunchReverse = False
def teleopPeriodic(self):
""" Limelight """
self.tx = self.dashboard.limelight(
'tx') # getting horizontal angle to target
self.ty = self.dashboard.limelight(
'ty') # getting vertical angle to target
self.dashboard.limelightHorizontalAngle(self.tx)
self.distance = self.vision.getDistance(self.ty)
self.dashboard.distance(
self.distance) # displaying distance from target to dashboard
if 80 < self.distance < 200:
self.dashboard.distanceStatus(True)
else:
self.dashboard.distanceStatus(False)
""" Color sensor - proximity sensor """
self.colorSensorProximity = self.colorSensor.getProximity()
self.dashboard.colorSensor(self.colorSensorProximity)
self.dashboard.ballsObtained(self.ballsInPossession)
# send RPM of shooter
self.dashboard.shooterRPMStatus(self.shooter.getShooterRPM('Top'),
self.shooter.getShooterRPM('Bottom'))
# gear status
self.dashboard.gearStatus(self.drive.gearSolenoid.get())
# auto align status
self.dashboard.autoAlign()
self.dashboard.autoRPM(self.setpointReached)
# turns limit switch on or off based on dashboard status
if self.dashboard.limitSwitchToggle() is True:
self.limitSwitchOverride = True
elif self.dashboard.limitSwitchToggle() is False:
self.limitSwitchOverride = False
# set shooter PID
self.shooter.setVarPID(self.dashboard.getTestValues('P Top'),
self.dashboard.getTestValues('I Top'),
self.dashboard.getTestValues('D Top'),
self.dashboard.getTestValues('F Top'), 'Top')
self.shooter.setVarPID(self.dashboard.getTestValues('P Bottom'),
self.dashboard.getTestValues('I Bottom'),
self.dashboard.getTestValues('D Bottom'),
self.dashboard.getTestValues('F Bottom'),
'Bottom')
self.shooter.setPID('Top')
self.shooter.setPID('Bottom')
# drive train encoders
# self.driveTrainEncoder = (self.drive.rearLeftEncoder.getSelectedSensorPosition() + self.drive.rearRightEncoder.getSelectedSensorPosition()) / 2
# self.dashboard.putDiagnosticValues('Drive Train Left Encoder', self.drive.rearLeftEncoder.getSelectedSensorPosition())
# self.dashboard.putDiagnosticValues('Drive Train Right Encoder', self.drive.rearRightEncoder.getSelectedSensorPosition())
# self.dashboard.putDiagnosticValues('Drive Train Encoder', self.driveTrainEncoder)
# sending lift state status to dashboard
self.dashboard.liftStatus(self.lift.liftSolenoid.get())
""" Pneumatics """
# compressor status
self.dashboard.compressorStatus(self.compressor.enabled())
# shooter rpm
self.shooterRPMTop = (abs(self.shooter.getShooterRPM('Top')))
self.shooterRPMBottom = (abs(self.shooter.getShooterRPM('Bottom')))
""" Drive """
# controller mapping for drive
self.driveLeft = self.leftJoystick.getRawAxis(1)
self.driveRight = self.rightJoystick.getRawAxis(1)
self.driveRotate = self.rightJoystick.getRawAxis(2)
# controller mapping for auto turn
self.autoTurnButton = self.xbox.getRawButton(6)
# changing between arcade and tank drive
if self.driveButtonStatus.on is True:
self.drive.arcadeDrive(self.driveRight, self.driveRotate)
# sending drive train driving mode to dashboard
self.dashboard.driveStatus('Arcade Drive')
elif self.driveButtonStatus.on is False:
self.drive.tankDrive(self.driveLeft, self.driveRight)
# sending drive train driving mode to dashboard
self.dashboard.driveStatus('Tank Drive')
# changing drive train gears
self.drive.changeGear(self.gearButtonStatus.get())
""" Lift """
# only runs lift if up
if self.lift.liftSolenoid.get() == 1:
self.lift.runMotor(self.xbox.getRawAxis(2))
# changing lift state
self.lift.changeLift(self.liftButtonStatus.get())
""" Shooter """
# controller mapping for shooter
self.shooterLaunch = self.xbox.getRawAxis(3) # right trigger
# sets shooter at a certain RPM if the trigger is being pressed
# if self.distance < 170:
# self.targetRPMTop = 3871 + (-39.3 * self.distance) + (0.138 * (self.distance * self.distance)) # 4079 3400 + (-22.3 * self.distance) + (0.0576 * (self.distance * self.distance)) # 3759
# elif self.distance > 170:
# self.targetRPMTop = -3600 + (46.1 * self.distance) + (-0.101 * (self.distance * self.distance)) # -3330
self.targetRPMTop = self.dashboard.getTestValues('RPM Top')
self.targetRPMBottom = self.dashboard.getTestValues('RPM Bottom')
# send rpm to dashboard
self.dashboard.putDiagnosticValues('Top Shooter RPM',
self.shooter.getShooterRPM('Top'))
self.dashboard.putDiagnosticValues(
'Bottom Shooter RPM', self.shooter.getShooterRPM('Bottom'))
# sets shooter at a certain RPM if right trigger is being pressed
# self.targetRPMBottom = self.targetRPMTop * 2 # introducing backspin
self.shooter.setSetpoint('Top', self.targetRPMTop)
self.shooter.setSetpoint('Bottom', self.targetRPMBottom)
# self.dashboard.dashboard.putNumber("Top Error", self.shooter.rcwTop)
# self.dashboard.dashboard.putNumber("Bottom Error", self.shooter.rcwBottom)
self.setpointReached = False
if self.shooterLaunch > 0.25:
if self.shooterLaunchReverse is True:
self.timer.reset()
self.shooterLaunchReverse = False
# if self.timer.get() < 0.5:
# self.semicircle.run('Reverse')
self.shooterRun = True
self.dashboard.limelightLed(True)
self.shooter.execute('Top')
self.shooter.execute('Bottom')
self.ballsInPossession = 0
error = 10000 # allowing 100 RPM error
if (self.targetRPMTop - error) <= self.shooterRPMTop <= (self.targetRPMTop + error) and \
(self.targetRPMBottom - error) <= self.shooterRPMBottom <= (self.targetRPMBottom + error) and \
self.setpointReached is False:
self.setpointReached = True
else:
self.setpointReached = False
else:
self.shooterRun = False
self.shooter.topMotors.set(0)
self.shooter.bottomMotors.set(0)
""" Intake, Indexer, and Semicircle """
# controller mapping for intake, indexer, and semicircle
self.dpad = self.xbox.getPOV()
# checks for number of balls in possession in semicircle
if self.limitSwitch.get(
) is False and self.limitSwitchTriggered is False:
self.ballsInPossession += 1
self.limitSwitchTriggered = True
elif self.limitSwitch.get() is True:
self.limitSwitchTriggered = False
# changes variables depending on what dpad position is pressed
if self.dpad == 315 or self.dpad == 0 or self.dpad == 45: # top half of dpad
self.dpadForward = True
else:
self.dpadForward = False
if self.dpad == 135 or self.dpad == 180 or self.dpad == 225: # bottom half of dpad
self.dpadBackwards = True
else:
self.dpadBackwards = False
# uses 'forward', 'reverse', 'stop'
if self.dpadForward is True:
# ejects balls from intake
self.intake.run('Reverse')
self.indexer.flatIndexer.set(0.75)
self.semicircle.run('Stop')
elif self.dpadBackwards is True:
if self.colorSensorProximity >= self.colorSensitivity and self.ballsInPossession < 2:
# runs if a ball is detected
self.intake.run('Forward')
self.indexer.run('Forward')
self.semicircle.run('Forward')
elif self.colorSensorProximity < self.colorSensitivity and self.ballsInPossession < 2:
# runs if ball is not detected
self.intake.run('Forward')
self.indexer.run('Forward')
self.semicircle.run('Stop')
elif self.ballsInPossession >= 2:
# runs intake only if there are three or more balls in the semicircle
if self.limitSwitchOverride is False:
self.intake.run('Forward')
self.indexer.run('Stop')
self.semicircle.run('Stop')
elif self.limitSwitchOverride is True:
self.intake.run('Forward')
self.indexer.run('Forward')
self.semicircle.run('Forward')
elif self.shooterRun is True and self.setpointReached is False:
# runs while shooter is activated but not yet at target RPM
self.intake.run('Forward')
self.indexer.run('Stop')
self.semicircle.run('Stop')
elif self.shooterRun is True and self.setpointReached is True:
# runs while shooter is activated and target RPM is reached
self.intake.run('Forward')
self.indexer.run('Forward')
self.semicircle.run('Forward')
elif self.xbox.getRawButton(7) is True:
# the oh [insert four letter string] here
self.intake.intakeMotor.set(-1)
self.indexer.indexer.set(-1)
self.semicircle.semicircleMotor.set(-1)
elif self.xbox.getRawButton(8) is True:
self.intake.intakeMotor.set(1)
self.semicircle.semicircleMotor.set(1)
else:
self.intake.run('Stop')
self.indexer.run('Stop')
self.semicircle.run('Stop')
""" Auto Turn w/ NavX """
if self.xbox.getRawButton(6) is True: # right bumper
self.dashboard.limelightLed(True)
if self.drive.gearSolenoid == 2:
self.drive.changeGear(False) # changes to low gear
self.drive.turnToTarget(self.tx) # turn to limelight target
self.dashboard.dashboard.putBoolean(
"Auto Align",
True) # sets variable to reverse semicircle before shoot
# if self.drive.resetAngle == True:
# self.drive.reset()
# self.drive.turnAngle(self.tx)
# if self.drive.resetAngle == True:
# self.drive.reset()
else:
self.dashboard.dashboard.putBoolean("Auto Align", False)
self.dashboard.limelightLed(False)