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)