def createObjects(self):
"""Initialize all wpilib motors & sensors"""
wpilib.CameraServer.launch()
# LiveWindow slows down the robot, and we aren't using it
wpilib.LiveWindow.disableAllTelemetry()
self.mainStick = wpilib.Joystick(0)
self.extraStick = wpilib.Joystick(1)
self.armUp = False
self.drive_l1 = ctre.WPI_TalonSRX(1)
self.drive_l2 = ctre.VictorSPX(2)
self.drive_l3 = ctre.VictorSPX(3)
self.drive_r1 = ctre.WPI_TalonSRX(4)
self.drive_r2 = ctre.VictorSPX(5)
self.drive_r3 = ctre.VictorSPX(6)
self.elevator_motor1 = ctre.WPI_TalonSRX(7)
self.arm_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.cargo_intake_motor = ctre.WPI_TalonSRX(11)
self.hatch_intake_motor = ctre.WPI_TalonSRX(8)
self.shiftSolenoid1 = wpilib.DoubleSolenoid(0, 1)
self.shiftSolenoid2 = wpilib.DoubleSolenoid(3, 2)
self.blinkin = wpilib.Spark(1)
self.gear = 1
self.irSensor = SharpIR2Y0A21(0)
def createObjects(self):
self.joystickR = wpilib.Joystick(0)
self.drive_l1 = ctre.WPI_TalonSRX(1)
self.drive_l2 = ctre.VictorSPX(2)
self.drive_l3 = ctre.VictorSPX(3)
self.drive_r1 = ctre.WPI_TalonSRX(4)
self.drive_r2 = ctre.VictorSPX(5)
self.drive_r3 = ctre.VictorSPX(6)
def robotInit(self):
wpilib.CameraServer.launch()
wpilib.CameraServer.launch('vision.py:main')
self.flm = ctre.VictorSPX(2)
self.frm = ctre.VictorSPX(3)
self.frm.setInverted(True)
self.blm = ctre.VictorSPX(1)
self.brm = ctre.VictorSPX(4)
self.brm.setInverted(True)
self.flm.set(ControlMode.PercentOutput, 0)
self.frm.set(ControlMode.PercentOutput, 0)
self.blm.set(ControlMode.PercentOutput, 0)
self.brm.set(ControlMode.PercentOutput, 0)
def __init__(self):
self.leftMaster = ctre.TalonSRX(robotmap.LEFT_DRIVE_MASTER)
self.leftSlave = ctre.VictorSPX(robotmap.LEFT_DRIVE_SLAVE)
self.rightMaster = ctre.TalonSRX(robotmap.RIGHT_DRIVE_MASTER)
self.rightSlave = ctre.VictorSPX(robotmap.RIGHT_DRIVE_SLAVE)
self.rightTalon.setInverted(True)
self.leftTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.rightTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.leftSlave.follow(leftMaster)
self.rightSlave.follow(rightMaster)
def robotInit(self):
modes = Modes()
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.gyro = wpilib.interfaces.Gyro()
ds = wpilib.DriverStation()
mode = modes.Talon
if mode == modes.Talon:
self.frLeft = ctre.TalonSRX(2)
self.frRight = ctre.TalonSRX(3)
self.reLeft = ctre.TalonSRX(1)
self.reRight = ctre.TalonSRX(4)
elif mode == modes.Victor:
self.frLeft = ctre.VictorSPX(2)
self.frRight = ctre.VictorSPX(3)
self.reLeft = ctre.VictorSPX(1)
self.reRight = ctre.VictorSPX(4)
else:
self.frLeft = ctre.TalonSRX(2)
self.frRight = ctre.TalonSRX(3)
self.reLeft = ctre.TalonSRX(1)
self.reRight = ctre.TalonSRX(4)
self.controller = wpilib.XboxController(1)
self.timer = wpilib.Timer()
self.stick = wpilib.XboxController(1)
self.timer = wpilib.Timer()
self.frLeft.set(ctre.ControlMode.PercentOutput, 0)
self.reLeft.set(ctre.ControlMode.PercentOutput, 0)
self.frRight.set(ctre.ControlMode.PercentOutput, 0)
self.reRight.set(ctre.ControlMode.PercentOutput, 0)
self.gyro.calibrate()
self.gyro.reset()
# Set Inverted
self.frLeft.setInverted(False)
self.reLeft.setInverted(False)
self.frRight.setInverted(False)
self.reRight.setInverted(True)
def robotInit(self):
# Initializes the network table
# NetworkTables.initialize(server="10.56.13.2")
# self.table = NetworkTables.getTable("limelight")
# f is a control constant that will be used to change variable values quickly
# self.f = 1
# self.ControlConstant = -0.1 * self.f
# self.minCommand = -0.05 * self.f
# self.Blue = (RGB value or blue color)
# self.Green = (RGB value of green color)
# self.Red = (RGB value of red color)
# self.Yellow = (RGB value of Yellow color)
# Defines the Joystick that we will be using for driving.
self.DriveStick = wpilib.Joystick(self.DriveStickChannel)
# Initializing drive motors
self.FLMotor = wpilib.Spark(self.FLChannel)
self.FRMotor = wpilib.Spark(self.FRChannel)
self.RLMotor = wpilib.Spark(self.RLChannel)
self.RRMotor = wpilib.Spark(self.RRChannel)
self.LoaderGrab = ctre.VictorSPX(1)
# Puts the motors into groups so that they fit the parameters of the function.
self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor)
self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor)
# The drive function that tells the computer what kind of drive to use and where the motors are.
self.drive = DifferentialDrive(self.LMG, self.RMG)
# self.ColorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# Components is a dictionary that contains any variables you want to put into it. All of the variables put into
# components dictionary is given to the autonomous modes.
# self.components = {"drive": self.drive}
# Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous
# modes should inherit.
# self.automodes = autonomous.AutonomousModeSelector("autonomous", self.components)
self.timer = wpilib.Timer()
def createObjects(self):
# Drivetrain
self.motor_fl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][0],
rev.MotorType.kBrushless) # 11
self.motor_fr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][0],
rev.MotorType.kBrushless) # 15
self.motor_rl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][0],
rev.MotorType.kBrushless) # 13
self.motor_rr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][0],
rev.MotorType.kBrushless) # 17
self.motor_rr0.setInverted(True)
self.motor_fr0.setInverted(True)
# Set Motors to follow each other
# Ignore secondary motors for simulation, not fully supported yet
if not self.isSimulation():
self.motor_fr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][1],
rev.MotorType.kBrushless)
self.motor_fl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][1],
rev.MotorType.kBrushless)
self.motor_rl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][1],
rev.MotorType.kBrushless)
self.motor_rr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][1],
rev.MotorType.kBrushless)
self.motor_fl1.follow(self.motor_fl0)
self.motor_fr1.follow(self.motor_fr0)
self.motor_rl1.follow(self.motor_rl0)
self.motor_rr1.follow(self.motor_rr0)
self.drivetrain_train = wpilib.drive.MecanumDrive(
self.motor_fl0, self.motor_rl0, self.motor_fr0, self.motor_rr0)
self.drivetrain_gyro = wpilib.ADXRS450_Gyro()
# Elevator
self.elevator_motor1 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][0])
self.elevator_motor2 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][1])
self.elevator_motor2.follow(self.elevator_motor1)
self.elevator_limit_switch_bottom = wpilib.DigitalInput(
robotmap.ELEVATOR["lower"])
self.elevator_encoder = TalonEncoder(self.elevator_motor1)
# Hatch
self.hatch_hold_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["actuator"])
self.hatch_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["lift"])
# Cargo
self.cargo_motor = ctre.TalonSRX(robotmap.INTAKE["cargo"]["actuator"])
self.cargo_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["cargo"]["lift"])
# Climber
self.climber_front_left_motor = ctre.VictorSPX(
robotmap.CLIMBER_FL["motor"])
self.climber_front_right_motor = ctre.VictorSPX(
robotmap.CLIMBER_FR["motor"])
self.climber_back_motor = ctre.VictorSPX(
robotmap.CLIMBER_BACK["motor"])
self.climber_wheel_motor = ctre.VictorSPX(robotmap.CLIMBER_WHEELS)
# Climber Limit Switches
self.climber_front_left_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["top"])
self.climber_front_right_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["top"])
self.climber_front_left_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["bottom"])
self.climber_front_right_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["bottom"])
self.climber_back_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["top"])
self.climber_back_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["bottom"])
# Joystick 1
self.drive_joystick = wpilib.Joystick(0)
self.slow_button = JoystickButton(self.drive_joystick, 1)
self.bottom_tower_front_button = JoystickButton(self.drive_joystick, 9)
self.bottom_tower_back_button = JoystickButton(self.drive_joystick, 10)
self.perp_button = JoystickButton(self.drive_joystick, 6)
self.top_tower_front_button = JoystickButton(self.drive_joystick, 7)
self.top_tower_back_button = JoystickButton(self.drive_joystick, 8)
self.climb_button = JoystickButton(self.drive_joystick, 3)
self.climb_cancel_button = JoystickButton(self.drive_joystick, 4)
# Joystick 2
self.op_joystick = wpilib.Joystick(1)
self.tower_l1_button = JoystickButton(self.op_joystick, 1)
self.tower_l2_button = JoystickButton(self.op_joystick, 2)
self.tower_l3_button = JoystickButton(self.op_joystick, 3)
self.elevator_load_button = JoystickButton(self.op_joystick, 4)
self.hatch_panel_button = JoystickButton(self.op_joystick, 9)
self.cargo_ball_button = JoystickButton(self.op_joystick, 10)
self.defense_mode_button = JoystickButton(self.op_joystick, 5)
self.intake_button = JoystickButton(self.op_joystick, 5)
self.release_button = JoystickButton(self.op_joystick, 6)
# Climb Joystick
self.climb_joystick = wpilib.Joystick(2)
self.climber_front_lift_button = JoystickButton(self.climb_joystick, 1)
self.climber_back_lift_button = JoystickButton(self.climb_joystick, 2)
self.climber_front_lower_button = JoystickButton(
self.climb_joystick, 3)
self.climber_back_lower_button = JoystickButton(self.climb_joystick, 4)
self.climber_drive_button = JoystickButton(self.climb_joystick, 5)
self.climber_reverse_button = JoystickButton(self.climb_joystick, 6)
def createObjects(self):
"""Create motors and stuff here."""
# a + + b - + c - - d + -
x_dist = 0.2625
y_dist = 0.2665
self.module_a = SwerveModule( # front left module
"a",
steer_talon=ctre.TalonSRX(3),
drive_talon=ctre.TalonSRX(4),
x_pos=x_dist,
y_pos=y_dist,
)
self.module_b = SwerveModule( # front right module
"b",
steer_talon=ctre.TalonSRX(7),
drive_talon=ctre.TalonSRX(8),
x_pos=-x_dist,
y_pos=y_dist,
)
self.module_c = SwerveModule( # bottom left module
"c",
steer_talon=ctre.TalonSRX(1),
drive_talon=ctre.TalonSRX(6),
x_pos=-x_dist,
y_pos=-y_dist,
)
self.module_d = SwerveModule( # bottom right module
"d",
steer_talon=ctre.TalonSRX(23),
drive_talon=ctre.TalonSRX(24),
x_pos=x_dist,
y_pos=-y_dist,
)
self.imu = NavX()
wpilib.SmartDashboard.putData("Gyro", self.imu.ahrs)
# hatch objects
self.hatch_fingers = wpilib.DoubleSolenoid(7, 6)
self.hatch_punchers = wpilib.Solenoid(0)
self.hatch_enable_piston = wpilib.DoubleSolenoid(3, 2)
self.hatch_left_limit_switch = wpilib.DigitalInput(8)
self.hatch_right_limit_switch = wpilib.DigitalInput(9)
self.climber_front_motor = rev.CANSparkMax(10, rev.MotorType.kBrushless)
self.climber_back_motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.climber_front_podium_switch = wpilib.DigitalInput(4)
self.climber_back_podium_switch = wpilib.DigitalInput(5)
self.climber_drive_motor = ctre.TalonSRX(20)
self.climber_pistons = wpilib.DoubleSolenoid(forwardChannel=4, reverseChannel=5)
# cargo related objects
self.intake_motor = ctre.VictorSPX(9)
self.intake_switch = wpilib.DigitalInput(0)
self.arm_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Talon CAN motor init
'''
Talon closed loop control guide
https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html
Talon (CTRE) Python API documentation
https://robotpy.readthedocs.io/projects/ctre/en/latest/api.html
Talon example code (Java and C++)
https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages
## One-time setup for each Talon
Use the diagnostics tool in Windows to:
1. Update the FW
2. Set the ID number
3. Write the ID down somewhere (ideally, label the motor)
4. Use the Self-Test Snapshot and Plot to make sure the motor works
NOTE: MOTOR POSITION CONTROL IS EXTREMELY DANGEROUS RIGHT NOW
NEVER USE POSITION CONTROL WITHOUT CURRENT LIMIT
NEVER USE POSITION CONTROL WITHOUT SUPERVISION + E-STOP READINESS
Look into limit switches for motor auto-shutoff
'''
self.shooterCAN = ctre.TalonFX(6)
self.initTalonFX(self.shooterCAN)
self.shooterCANfollow = ctre.TalonFX(1)
self.initTalonFX(self.shooterCANfollow, inverted=True)
self.shooterCANfollow.set(mode=ctre.ControlMode.Follower, value=6)
self.shooterHoodCAN = ctre.TalonFX(5)
self.initTalonFX(self.shooterHoodCAN)
#self.initTalonFX(self.shooterHoodCAN, kF=0, kP=0.02, kI=0, inverted=True, enCurrentLimit=True)
#self.shooterHoodCAN.setSelectedSensorPosition(0, 0, 10)
#Get information from network tables
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
self.lime = NetworkTables.getTable("limelight")
#Set up all the motor controllers
self.leftDriveCAN = ctre.WPI_TalonFX(13)
# self.initTalonFX(self.leftDriveCAN)
self.leftDriveCANfollow = ctre.TalonFX(12)
# self.initTalonFX(self.leftDriveCANfollow)
self.leftDriveCANfollow.set(mode=ctre.ControlMode.Follower, value=13)
self.rightDriveCAN = ctre.WPI_TalonFX(11)
# self.initTalonFX(self.rightDriveCAN)
self.rightDriveCANfollow = ctre.TalonFX(10)
# self.initTalonFX(self.rightDriveCANfollow)
self.rightDriveCANfollow.set(mode=ctre.ControlMode.Follower, value=11)
self.indexerMotorCAN = ctre.VictorSPX(9)
self.intakeMotorCAN = ctre.VictorSPX(8)
self.leftClimbCAN = ctre.VictorSPX(15)
self.rightClimbCAN = ctre.VictorSPX(14)
#Set up the drivetrain motors.
self.drive = wpilib.drive.DifferentialDrive(self.leftDriveCAN,
self.rightDriveCAN)
self.intakeOn = False
self.indexerOn = False
self.shooterOn = False
self.hoodOn = False
self.cycleCount = 0
self.hoodDirection = 'stopped' #stopped, forward, backward
self.currentHoodPos = 0
self.hoodSlop = 0.005
self.zeroHood = False
self.hoodSpeed = .06
self.hoodCtsPerRot = 2048 * 70.0 # counts per rotation * gear reduction / quadrature?
self.joystick = wpilib.XboxController(0)
self.indexerTimer = wpilib.Timer()
self.indexerTimer.start()
self.shooterTimer = wpilib.Timer()
self.shooterTimer.start()
self.hoodTimer = wpilib.Timer()
self.hoodTimer.start()