def robotInit(self):
self.control_stick = wpilib.Joystick(0)
self.open_claw = ButtonDebouncer(self.control_stick, 1)
self.close_claw = ButtonDebouncer(self.control_stick, 2)
self.claw = claw.Claw(2, 0)
def __init__(self, robot, control_stick):
self.robot = robot
self.stick = control_stick
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 7)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
def __init__(self, robot):
self.robot = robot
self.stick = wpilib.Joystick(0)
self.throttle = wpilib.Joystick(1)
self.claw_const_pressure_active = False
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 2)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
#self.lf_motor = wpilib.Victor(0b00) # => 0
#self.lr_motor = wpilib.Victor(0b01) # => 1
#self.rf_motor = wpilib.Victor(0b10) # => 2
#self.rr_motor = wpilib.Victor(0b11) # => 3
# TODO: This is not in any way an ideal numbering system.
# The PWM ports should be redone to use the old layout above.
self.lf_motor = wpilib.Victor(9)
self.lr_motor = wpilib.Victor(8)
self.rf_motor = wpilib.Victor(7)
self.rr_motor = wpilib.Victor(6)
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(
self.intake_wheel_left, self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def createObjects(self):
wpilib.CameraServer.launch()
wpilib.LiveWindow.disableAllTelemetry()
self.left_drive_motor = WPI_TalonSRX(0)
WPI_TalonSRX(1).set(WPI_TalonSRX.ControlMode.Follower,
self.left_drive_motor.getDeviceID())
self.right_drive_motor = WPI_TalonSRX(2)
WPI_TalonSRX(3).set(WPI_TalonSRX.ControlMode.Follower,
self.right_drive_motor.getDeviceID())
self.robot_drive = wpilib.drive.DifferentialDrive(
self.left_drive_motor, self.right_drive_motor)
self.r_intake_motor = WPI_VictorSPX(4)
self.l_intake_motor = WPI_VictorSPX(5)
self.elevator_winch = WPI_TalonSRX(6)
self.climber_motor = WPI_TalonSRX(7)
self.wrist_motor = WPI_TalonSRX(8)
self.intake_ir = wpilib.AnalogInput(0)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 3)
self.right_drive_joystick = wpilib.Joystick(0)
self.left_drive_joystick = wpilib.Joystick(1)
self.operator_joystick = wpilib.Joystick(2)
self.compressor = wpilib.Compressor()
self.elevator_limit_switch = wpilib.DigitalInput(0)
self.climber_motor = WPI_TalonSRX(7)
self.navx = AHRS.create_spi()
self.path_tracking_table = NetworkTables.getTable("path_tracking")
self.down_button = ButtonDebouncer(self.operator_joystick, 1)
self.right_button = ButtonDebouncer(self.operator_joystick, 2)
self.left_button = ButtonDebouncer(self.operator_joystick, 3)
self.has_cube_button = ButtonDebouncer(self.operator_joystick, 5)
self.up_button = ButtonDebouncer(self.operator_joystick, 4)
self.left_bumper_button = JoystickButton(self.operator_joystick, 5)
self.right_bumper_button = JoystickButton(self.operator_joystick, 6)
def robotInit(self):
self.stick = wpilib.Joystick(self.controller_index)
self.__prefs = wpilib.Preferences.getInstance()
self.lift_main = TalonSRX(self.main_lift_id)
self.lift_follower = TalonSRX(self.follower_id)
self.lift_main.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.PulseWidthEncodedPosition, 0, 0)
self.lift_main.selectProfileSlot(0, 0)
self.lift_follower.set(TalonSRX.ControlMode.Follower,
self.main_lift_id)
self.last_out = 0
self.back = ButtonDebouncer(self.stick, 2)
self.fwd = ButtonDebouncer(self.stick, 3)
self.__load_config()
def createObjects(self):
self.leftStick = wpilib.Joystick(2)
self.elevatorStick = wpilib.Joystick(1)
self.rightStick = wpilib.Joystick(0)
self.elevatorMotorOne = wpilib.Victor(2)
self.elevatorMotorTwo = wpilib.Victor(3)
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.myRobot = DifferentialDrive(self.left, self.right)
self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne,
self.elevatorMotorTwo)
self.elevatorPot = wpilib.AnalogPotentiometer(0)
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.gearshift.set(1)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # => 0
self.lr_motor = wpilib.Victor(0b01) # => 1
self.rf_motor = wpilib.Victor(0b10) # => 2
self.rr_motor = wpilib.Victor(0b11) # => 3
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
def createObjects(self):
# Joysticks
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# Motors (l/r = left/right, f/r = front/rear)
self.lf_motor = wpilib.CANTalon(5)
self.lr_motor = wpilib.CANTalon(10)
self.rf_motor = wpilib.CANTalon(15)
self.rr_motor = wpilib.CANTalon(20)
# Drivetrain object
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
# Left and right arm motors (there's two, which both control the raising and lowering the arm)
self.leftArm = wpilib.CANTalon(25)
self.rightArm = wpilib.CANTalon(30)
# Motor that spins the bar at the end of the arm.
# There was originally going to be one on the right, but we decided against that in the end.
# In retrospect, that was probably a mistake.
self.leftBall = wpilib.Talon(9)
# Motor that reels in the winch to lift the robot.
self.winchMotor = wpilib.Talon(0)
# Motor that opens the winch.
self.kickMotor = wpilib.Talon(1)
# Aiming flashlight
self.flashlight = wpilib.Relay(0)
# Timer to keep light from staying on for too long
self.lightTimer = wpilib.Timer()
# Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off.
# self.turningOffState keeps track of which on/off it's on.
self.turningOffState = 0
# Is currently on or off? Used to detect if UI button is pressed.
self.lastState = False
# Drive encoders; measure how much the motor has spun
self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True)
self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor)
# Distance sensors
self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0)
self.ultrasonic = wpilib.AnalogInput(1)
# NavX (purple board on top of the RoboRIO)
self.navX = navx.AHRS.create_spi()
# Initialize SmartDashboard, the table of robot values
self.sd = NetworkTable.getTable('SmartDashboard')
# How much will the control loop pause in between (0.025s = 25ms)
self.control_loop_wait_time = 0.025
# Button to reverse controls
self.reverseButton = ButtonDebouncer(self.joystick1, 1)
# Initiate functional buttons on joysticks
self.shoot = ButtonDebouncer(self.joystick2, 1)
self.raiseButton = ButtonDebouncer(self.joystick2, 3)
self.lowerButton = ButtonDebouncer(self.joystick2, 2)
self.lightButton = ButtonDebouncer(self.joystick1, 6)
def createObjects(self):
"""
Create basic components (motor controllers, joysticks, etc.).
"""
# NavX
self.navx = navx.AHRS.create_spi()
# Initialize SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
# Joysticks
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.secondary_joystick = wpilib.Joystick(2)
# Triggers and buttons
self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1)
# Drive motors
self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True)
self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True)
self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59)
self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True)
# Drive control
self.field_centric_button = ButtonDebouncer(self.left_joystick, 6)
self.predict_position = ButtonDebouncer(self.left_joystick, 7)
self.field_centric_drive = True
self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1)
self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4)
self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5)
self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10)
# Shooting motors
self.shooter_motor = ctre.CANTalon(15)
self.belt_motor = wpilib.spark.Spark(9)
self.light_controller = wpilib.VictorSP(8)
# Pistons for gear picker
self.picker = wpilib.DoubleSolenoid(6, 7)
self.pivot = wpilib.DoubleSolenoid(4, 5)
self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0))
# Toggling button on secondary joystick
self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2)
# Or, up and down buttons on right joystick
self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2)
self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3)
# Climb motors
self.climb_motor1 = wpilib.spark.Spark(4)
self.climb_motor2 = wpilib.spark.Spark(5)
# Camera gimble
self.gimbal_yaw = wpilib.Servo(6)
self.gimbal_pitch = wpilib.Servo(7)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
def robotInit(self):
self.kSlotIdx = 0
self.kPIDLoopIdx = 0
self.kTimeoutMs = 10
# Sets the speed
self.speed = 0.4
self.ySpeed = 1
self.tSpeed = 0.75
self.baseIntakeSpeed = 5
self.previousAvg = 0
self.currentAvg = 0
# Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
# joysticks 1 & 2 on the driver station
self.driverJoystick = wpilib.Joystick(0)
# Create a simple timer (docs: https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib/Timer.html#wpilib.timer.Timer.get)
self.timer = wpilib.Timer()
# TODO: Fix module number
self.compressor = wpilib.Compressor()
# TODO: Fix module numbers
self.intake = Intake(0, 0, 0, 0, 0, 0, 0)
# Talon CAN devices
# TODO: Fix module numbers
self.frontLeftTalon = WPI_TalonSRX(2)
self.rearLeftTalon = WPI_TalonSRX(0)
self.frontRightTalon = WPI_TalonSRX(3)
self.rearRightTalon = WPI_TalonSRX(1)
self.rightPistonButton = ButtonDebouncer(driverJoystick, 4)
self.leftPistonButton = ButtonDebouncer(driverJoystick, 5)
# Enable auto breaking
self.frontLeftTalon.setNeutralMode(NeutralMode.Brake)
self.rearLeftTalon.setNeutralMode(NeutralMode.Brake)
self.frontRightTalon.setNeutralMode(NeutralMode.Brake)
self.rearRightTalon.setNeutralMode(NeutralMode.Brake)
# Setup encoders
self.frontLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.frontRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# Setup encoders
self.leftEncoder = self.rearLeftTalon
self.rightEncoder = self.rearRightTalon
def createObjects(self):
"""
Initialize robot components.
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_claw = ButtonDebouncer(self.joystick_left, 1)
self.btn_forearm = ButtonDebouncer(self.joystick_right, 1)
self.btn_up = JoystickButton(self.joystick_left, 3)
self.btn_down = JoystickButton(self.joystick_left, 2)
self.btn_climb = JoystickButton(self.joystick_right, 11)
# Buttons on alternative joystick
self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1)
self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2)
self.btn_climb_alt = JoystickButton(self.joystick_alt, 3)
# Buttons for toggling control options and aides
self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8)
self.btn_record = ButtonDebouncer(self.joystick_left, 6)
self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12)
self.btn_fine_movement = JoystickButton(self.joystick_right, 2)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Follow front wheels with rear to save CAN packets
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8))
# Motion Profiling
self.position_controller = motion_profile.PositionController()
# Arm
self.elevator = wpilib.Victor(5)
self.forearm = wpilib.DoubleSolenoid(2, 3)
self.claw = wpilib.DoubleSolenoid(0, 1)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
self.ds = wpilib.DriverStation.getInstance()
self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
def createObjects(self):
"""
Initialize robot components.
"""
# For using teleop in autonomous
self.robot = self
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.button_strafe_left = JoystickButton(self.joystick_left, 4)
self.button_strafe_right = JoystickButton(self.joystick_left, 5)
self.button_strafe_forward = JoystickButton(self.joystick_left, 3)
self.button_strafe_backward = JoystickButton(self.joystick_left, 2)
self.button_slow_rotation = JoystickButton(self.joystick_right, 4)
self.button_lift_actuate = ButtonDebouncer(self.joystick_alt, 2)
self.button_manual_lift_control = ButtonDebouncer(self.joystick_alt, 6)
self.button_hatch_kick = JoystickButton(self.joystick_alt, 1)
self.button_cargo_push = JoystickButton(self.joystick_alt, 5)
self.button_cargo_pull = JoystickButton(self.joystick_alt, 3)
self.button_cargo_pull_lightly = JoystickButton(self.joystick_alt, 4)
self.button_climb_front = JoystickButton(self.joystick_right, 3)
self.button_climb_back = JoystickButton(self.joystick_right, 2)
self.button_target = JoystickButton(self.joystick_right, 8)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) *
self.WHEEL_DIAMETER * math.pi)
self.r_encoder = wpilib.Encoder(0, 1)
self.r_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder = wpilib.Encoder(2, 3)
self.l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder.setReverseDirection(True)
# Drivetrain
self.train = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
# Functional motors
self.lift_motor = WPI_TalonSRX(40)
self.lift_motor.setSensorPhase(True)
self.lift_switch = wpilib.DigitalInput(4)
self.lift_solenoid = wpilib.DoubleSolenoid(2, 3)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.left_cargo_intake_motor = WPI_TalonSRX(35)
# TODO: electricians soldered one motor in reverse.
# self.left_cargo_intake_motor.setInverted(True)
self.right_cargo_intake_motor = WPI_TalonSRX(30)
"""
self.cargo_intake_motors = wpilib.SpeedControllerGroup(self.left_cargo_intake_motor,
self.right_cargo_intake_motor)
"""
self.right_cargo_intake_motor.follow(self.left_cargo_intake_motor)
self.front_climb_piston = wpilib.DoubleSolenoid(4, 5)
self.back_climb_piston = wpilib.DoubleSolenoid(6, 7)
# Tank Drivetrain
"""
self.tank_train = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
"""
# Load trajectories
self.generated_trajectories = load_trajectories()
# NavX
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
# self.ds = wpilib.DriverStation.getInstance()
# self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
wpilib.LiveWindow.disableAllTelemetry()
