def __init__(self):
super().__init__()
self.logger = logging.getLogger(self.getName())
# Configure motors
self.leftMasterTalon = WPI_TalonSRX(RobotMap.LEFT_MASTER_TALON)
self.leftSlaveTalon = WPI_TalonSRX(RobotMap.LEFT_SLAVE_TALON)
self.rightMasterTalon = WPI_TalonSRX(RobotMap.RIGHT_MASTER_TALON)
self.rightSlaveTalon = WPI_TalonSRX(RobotMap.RIGHT_SLAVE_TALON)
self.leftSlaveTalon.follow(self.leftMasterTalon)
self.rightSlaveTalon.follow(self.rightMasterTalon)
self.leftMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.rightMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.leftMasterTalon.setInverted(True)
self.leftSlaveTalon.setInverted(True)
self.configureDrivePID()
# Configure shift solenoid
self.shiftSolenoid = DoubleSolenoid(RobotMap.SHIFT_IN_SOLENOID,
RobotMap.SHIFT_OUT_SOLENOID)
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.rightTalon = WPI_TalonSRX(INTAKE_RIGHT_MOTOR)
self.leftTalon = WPI_TalonSRX(INTAKE_LEFT_MOTOR)
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)
# Set the front motors to be the followers of the rear motors
self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_RIGHT_MOTOR)
# Add the motors to the speed controller groups and create the differential drivetrain
self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
self.rightDrive = SpeedControllerGroup(self.frontRight,
self.rightTalon)
self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)
# Setup the default motor controller setup
self.initControllerSetup()
# Map the pigeon. This will be connected to an unused Talon.
self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
self.pigeonIMU = PigeonIMU(self.talonPigeon)
def createObjects(self):
"""
Initialize testbench components.
"""
self.joystick = wpilib.Joystick(0)
self.brushless = wpilib.NidecBrushless(9, 9)
self.spark = wpilib.Spark(4)
self.lf_victor = wpilib.Victor(0)
self.lr_victor = wpilib.Victor(1)
self.rf_victor = wpilib.Victor(2)
self.rr_victor = wpilib.Victor(3)
self.lf_talon = WPI_TalonSRX(5)
self.lr_talon = WPI_TalonSRX(10)
self.rf_talon = WPI_TalonSRX(15)
self.rr_talon = WPI_TalonSRX(20)
self.drive = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_victor, self.lr_victor,
self.lf_talon, self.lr_talon),
wpilib.SpeedControllerGroup(self.rf_victor, self.rr_victor,
self.rf_talon, self.rr_talon))
wpilib.CameraServer.launch()
def createObjects(self):
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
def __init__(self):
super().__init__(p=0.015, i=0.0001, d=0.0)
self._deadband = 0.1
self._turnOutput = 0.0
# Configure motors
motors = [
WPI_TalonSRX(i)
for i in [RM.DRIVE_LF, RM.DRIVE_LB, RM.DRIVE_RF, RM.DRIVE_RB]
]
for i, motor in enumerate(motors):
# motor.configFactoryDefault()
motor.enableVoltageCompensation(True)
motor.configOpenLoopRamp(1.4, 10)
motor.setNeutralMode(NeutralMode.Brake)
# Invert left side motors?
# if i <= 1:
# motor.setInverted(True)
# Set up PIDSubsystem parameters
self.setInputRange(0.0, 360.0)
self.pidController = self.getPIDController()
self.pidController.setContinuous(True)
self.pidController.setAbsoluteTolerance(1.0)
self.setSetpoint(0.0)
# Enable this is you use the PID functionality
# self.pidController.enable()
self.drive = MecanumDrive(*motors)
self.drive.setExpiration(1)
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0.1)
def createObjects(self):
"""
Initialize all wpilib motors & sensors
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
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_VictorSPX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_VictorSPX(25)
self.lf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.rf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
# Following masters
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drive init
self.train = wpilib.drive.DifferentialDrive(
self.lf_motor, self.rf_motor
)
# Arm
self.arm_motor = WPI_TalonSRX(0)
self.arm_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.arm_limit = wpilib.DigitalInput(4)
# Gyro
self.gyro = navx.AHRS.create_spi()
self.gyro.reset()
self.control_loop_wait_time = 0.02
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.talon = WPI_TalonSRX(BOOM_MOTOR)
# Setup the default motor controller setup
self.initOpenLoop()
self.initClosedLoop()
def __init__(self):
super().__init__()
self.leftIntakeTalon = WPI_TalonSRX(RobotMap.LEFT_INTAKE_TALON)
self.rightIntakeTalon = WPI_TalonSRX(RobotMap.RIGHT_INTAKE_TALON)
self.intakeVictor = WPI_VictorSPX(RobotMap.END_EFFECTOR_VICTOR)
self.intakeSolenoid = DoubleSolenoid(RobotMap.INTAKE_IN_SOLENOID,
RobotMap.INTAKE_OUT_SOLENOID)
self.placingSolenoid = DoubleSolenoid(RobotMap.PLACE_IN_SOLENOID,
RobotMap.PLACE_OUT_SOLENOID)
self.leftIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.rightIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.setPosition(IntakeOutput.Position.UP)
self.setArmPower(0)
self.mode = None # This cannot be set here because the buttons do not yet exist.
def __init__(self):
super().__init__("Drivetrain")
self.leftleader = WPI_TalonSRX(CAN_LEFT_LEADER)
set_motor(self.leftleader, WPI_TalonSRX.NeutralMode.Brake, False)
self.leftfollower = WPI_VictorSPX(CAN_LEFT_FOLLOWER)
set_motor(self.leftfollower, WPI_VictorSPX.NeutralMode.Brake, False)
self.leftfollower.follow(self.leftleader)
self.rightleader = WPI_TalonSRX(CAN_RIGHT_LEADER)
set_motor(self.rightleader, WPI_TalonSRX.NeutralMode.Brake, True)
self.rightfollower = WPI_VictorSPX(CAN_RIGHT_FOLLOWER)
set_motor(self.rightfollower, WPI_VictorSPX.NeutralMode.Brake, True)
self.rightfollower.follow(self.rightleader)
self.DS = wpilib.DoubleSolenoid(0, 1)
self.drive = DifferentialDrive(self.leftleader, self.rightleader)
self.drive.maxOutput = 1.0
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 __init__(self):
super().__init__()
self.liftDrive = WPI_TalonSRX(RobotMap.BACK_DRIVE_TALON)
self.liftSpark = CANSparkMax(RobotMap.LIFT_SPARK, MotorType.kBrushless)
self.liftSpark.restoreFactoryDefaults()
self.liftSpark.setIdleMode(IdleMode.kCoast)
self.liftSpark.setSmartCurrentLimit(40)
self.liftEncoder = self.liftSpark.getEncoder()
self.liftPID = self.liftSpark.getPIDController()
self.setLiftPIDConstants()
self.elevatorSpark = CANSparkMax(RobotMap.ELEVATOR_SPARK,
MotorType.kBrushless)
self.elevatorSpark.restoreFactoryDefaults()
self.elevatorSpark.setIdleMode(IdleMode.kCoast)
self.elevatorSpark.setInverted(True)
self.liftSpark.setSmartCurrentLimit(60)
self.elevatorEncoder = self.elevatorSpark.getEncoder()
self.elevatorPID = self.elevatorSpark.getPIDController()
self.setElevatorPIDConstants()
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()