def __init__(self, distance_per_pulse, robot): super().__init__() left_motors_instance = Left_Motors() right_motors_instance = Right_Motors() self.left_motors = left_motors_instance.left_motor_group self.right_motors = right_motors_instance.right_motor_group self.left_shifter = wpilib.Solenoid(0) self.right_shifter = wpilib.Solenoid(1) self.left_encoder = wpilib.Encoder(2,3) self.right_encoder = wpilib.Encoder(4,5) self.left_encoder.setDistancePerPulse(distance_per_pulse) self.right_encoder.setDistancePerPulse(distance_per_pulse) self.gyro = wpilib.ADXRS450_Gyro() self.robot_instance = robot self.drive = DifferentialDrive(self.left_motors, self.right_motors) self.gyro.reset() self.x = 0 self.y = 0 self.heading = math.pi/2 self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0
def robotInit(self):
"""Robot initialization function"""
self.gyro = wpilib.ADXRS450_Gyro()
self.gyro.reset()
self.gyro.calibrate()
self.modules = [
SwerveModule(
0.8 / 2 - 0.125,
0.75 / 2 - 0.1,
CANSparkMax(9, MotorType.kBrushless),
ctre.WPI_TalonFX(3),
steer_reversed=False,
drive_reversed=True,
),
SwerveModule(
-0.8 / 2 + 0.125,
-0.75 / 2 + 0.1,
CANSparkMax(7, MotorType.kBrushless),
ctre.WPI_TalonFX(5),
steer_reversed=False,
drive_reversed=True,
),
]
self.kinematics = SwerveDrive2Kinematics(self.modules[0].translation,
self.modules[1].translation)
self.joystick = wpilib.Joystick(0)
self.spin_rate = 1.5
def robotInit(self):
"""Robot-wide initialization code should go here"""
# Basic robot chassis setup
self.stick = wpilib.Joystick(0)
self.robot_drive = wpilib.RobotDrive(0, 1)
# Position gets automatically updated as robot moves
self.gyro = wpilib.ADXRS450_Gyro()
# Use PIDController to control angle
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.pidGet,
output=self.pidWrite)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
self.i = 0
self.sd = NetworkTables.getTable("SmartDashboard")
self.hephestus = NetworkTables.getTable("hephestus")
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name = "drivetrain")
self.robot = robot
# The digital gyro plugged into the SPI port on RoboRIO
self.gyro = wpilib.ADXRS450_Gyro()
# Motors used for driving
self.left = rev.CANSparkMax(1, rev.CANSparkMax.MotorType.kBrushless)
self.leftB = rev.CANSparkMax(3, rev.CANSparkMax.MotorType.kBrushless)
self.right = rev.CANSparkMax(2, rev.CANSparkMax.MotorType.kBrushless)
self.rightB = rev.CANSparkMax(4, rev.CANSparkMax.MotorType.kBrushless)
# TODO: switch to DifferentialDrive is the main object that deals with driving
self.drive = DifferentialDrive(self.left, self.right)
#TODO: These probably will not be the actual ports used
self.left_encoder = wpilib.Encoder(2, 3)
self.right_encoder = wpilib.Encoder(4, 5)
# Encoders may measure differently in the real world and in
# simulation. In this example the robot moves 0.042 barleycorns
# per tick in the real world, but the simulated encoders
# simulate 360 tick encoders. This if statement allows for the
# real robot to handle this difference in devices.
# TODO: Measure our encoder's distance per pulse
if robot.isReal():
self.left_encoder.setDistancePerPulse(0.042)
self.right_encoder.setDistancePerPulse(0.042)
else:
# Circumference in ft = 4in/12(in/ft)*PI
self.left_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
self.right_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(1)
# self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(2)
self.frontRightMotor = wpilib.Spark(3)
# self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(4)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = ''
# joysticks 1 & 2 on the driver station
self.Stick1 = wpilib.XboxController(0)
self.Stick2 = wpilib.Joystick(1)
self.aSolenoidLow = wpilib.DoubleSolenoid(2,3)
self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1)
self.iSolenoid = wpilib.DoubleSolenoid(4,5)
self.gyro = wpilib.ADXRS450_Gyro()
def __init__(self, robot):
self.robot = robot
# get drive motors
self.frontLeftCim = ctre.WPI_TalonSRX(robotmap.frontLeftDrive)
self.frontRightCim = ctre.WPI_TalonSRX(robotmap.frontRightDrive)
self.backLeftCim = ctre.WPI_TalonSRX(robotmap.backLeftDrive)
self.backRightCim = ctre.WPI_TalonSRX(robotmap.backRightDrive)
# configure motors
self.configureMotorCurrent(self.frontLeftCim)
self.configureMotorCurrent(self.frontRightCim)
self.configureMotorCurrent(self.backLeftCim)
self.configureMotorCurrent(self.backRightCim)
# reverse left side motors
self.frontLeftCim.setInverted(True)
self.backLeftCim.setInverted(True)
# make drivetrain
self.drivetrain = mecanumdrive.MecanumDrive(self.frontLeftCim, self.backLeftCim, self.frontRightCim, self.backRightCim)
# setup encoders
self.encoderLeft = wpilib.Encoder(aChannel=robotmap.leftEncoderChannelA, bChannel=robotmap.leftEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X)
self.encoderLeft.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.encoderLeft.setDistancePerPulse((6*math.pi)/(256))
self.encoderRight = wpilib.Encoder(aChannel=robotmap.rightEncoderChannelA, bChannel=robotmap.rightEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X)
self.encoderRight.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.encoderRight.setDistancePerPulse((6*math.pi)/(256))
# setup gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
super().__init__()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Initialize Networktables
self.sd = NetworkTables.getTable('SmartDashboard')
#Set up motors to drive robot
self.M2 = wpilib.VictorSP(2)
self.M3 = wpilib.VictorSP(3)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M2, self.M3)
self.M0 = wpilib.VictorSP(0)
self.M1 = wpilib.VictorSP(1)
self.right = wpilib.SpeedControllerGroup(self.M0, self.M1)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
#Camera
wpilib.CameraServer.launch()
#Servo
self.SV1 = wpilib.Servo(9)
self.SV2 = wpilib.Servo(8)
#Dashboard
NetworkTables.initialize(server='10.61.62.2')
#Switches
self.SW0 = wpilib.DigitalInput(0)
self.SW1 = wpilib.DigitalInput(1)
#Elevator
self.E = wpilib.VictorSP(5)
self.prepareCubeFlag = 0
self.grabCubeFlag = 0
self.deliverCubeFlag = 0
self.adjustLeftFlag = 0
self.adjustRightFlag = 0
self.driveFlag = 0
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.reset()
#All possible autonomous routines in a sendable chooser
'''
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("None", '4')
self.chooser.addObject("left-LeftScale", '1')
self.chooser.addObject("Middle-LeftScale", '2')
self.chooser.addObject("Right-LeftScale", '3')
self.chooser.addObject("Left-RightScale", '5')
'''
#wpilib.SmartDashboard.putData('Choice', self.chooser)
#Encoders
self.EC1 = wpilib.Encoder(2, 3)
self.EC2 = wpilib.Encoder(4, 5)
self.EC1.reset()
self.EC2.reset()
def createObjects(self):
"""
Create motors and stuff here
"""
# drivetrain motors
self.right_front_motor = ctre.WPI_TalonSRX(robotmap.right_front_drive)
self.right_back_motor = ctre.WPI_TalonSRX(robotmap.right_back_drive)
self.left_front_motor = ctre.WPI_TalonSRX(robotmap.left_front_drive)
self.left_back_motor = ctre.WPI_TalonSRX(robotmap.left_back_drive)
# configure motors - current limit, ramp rate, etc.
MotorConfigurator.bulk_config_drivetrain(self.right_front_motor,
self.right_back_motor,
self.left_front_motor,
self.left_back_motor)
# create motor groups based on side
self.left_drive_motors = wpilib.SpeedControllerGroup(
self.left_back_motor, self.left_front_motor)
self.right_drive_motors = wpilib.SpeedControllerGroup(
self.right_front_motor, self.right_back_motor)
# encoders
self.left_encoder = wpilib.Encoder(
aChannel=robotmap.left_encoder_a,
bChannel=robotmap.left_encoder_b,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.left_encoder.setPIDSourceType(
wpilib.Encoder.PIDSourceType.kDisplacement)
self.right_encoder = wpilib.Encoder(
aChannel=robotmap.right_encoder_a,
bChannel=robotmap.right_encoder_b,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.right_encoder.setPIDSourceType(
wpilib.Encoder.PIDSourceType.kDisplacement)
# create drivetrain based on groupings
self.drive = wpilib.drive.DifferentialDrive(self.left_drive_motors,
self.right_drive_motors)
# ahrs gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.calibrate()
# oi class
self.oi = OI.OI()
# launch automatic camera capturing for main drive cam
# TODO Mount camera
wpilib.CameraServer.launch()
# launch arduino code and start data server
self.arduino_server = ArduinoServer()
self.arduino_server.startServer()
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def __init__(self):
self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(frontLeftTalonSRX)
self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(rearLeftTalonSRX)
self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
frontRightTalonSRX)
self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(rearRightTalonSRX)
self.gyro = wpilib.ADXRS450_Gyro()
self.mecanumDrive = wpilib.drive.MecanumDrive(self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor)
def createObjects(self):
self.fl_motor = CANTalon(0)
self.fr_motor = CANTalon(1)
self.bl_motor = CANTalon(2)
self.br_motor = CANTalon(3)
self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.fr_motor,
self.bl_motor, self.br_motor)
self.drivetrain_gyro = wpilib.ADXRS450_Gyro()
self.bunny_motor = wpilib.Talon(0)
self.flipper_motor = wpilib.Talon(1)
self.drive_joystick = wpilib.Joystick(0)
self.operator_joystick = wpilib.Joystick(1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7)
self.fr_motor.setInverted(True)
self.br_motor.setInverted(True)
self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.bl_motor, self.fr_motor, self.br_motor)
self.joystick = wpilib.Joystick(0)
self.JOYSTICK_DEADZONE_THRESHOLD = .5
#self.gyro = navx.AHRS.create_spi()
self.gyro = wpilib.ADXRS450_Gyro()
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = ctre.WPI_VictorSPX(1) #motor initialization
self.leftMotor2 = ctre.WPI_VictorSPX(3)
self.leftMotor3 = ctre.WPI_VictorSPX(5)
self.rightMotor1 = ctre.WPI_VictorSPX(0)
self.rightMotor2 = ctre.WPI_VictorSPX(2)
self.rightMotor3 = ctre.WPI_VictorSPX(4)
self.armMotor = ctre.WPI_VictorSPX(6)
self.leftIntakeMotor = ctre.WPI_VictorSPX(7)
self.rightIntakeMotor = ctre.WPI_VictorSPX(8)
self.leftSide = wp.SpeedControllerGroup(
self.leftMotor1, self.leftMotor2,
self.leftMotor3) #speed controller groups
self.rightSide = wp.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2,
self.rightMotor3)
self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor,
self.rightIntakeMotor)
self.myRobot = DifferentialDrive(self.leftSide, self.rightSide)
self.myRobot.setExpiration(0.1)
self.leftStick = wp.Joystick(0)
self.rightStick = wp.Joystick(1)
self.armStick = wp.Joystick(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.compressor = wp.Compressor()
wp.SmartDashboard.putNumber("Straight Position", 4000)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Turn Auto", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.ADXRS450_Gyro()
self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
def __init__(self):
'''Instantiates the drivetrain object.'''
super().__init__('Drivetrain')
self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
RobotMap.drivetrain.frontLeftMotor)
self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
RobotMap.drivetrain.frontRightMotor)
self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
RobotMap.drivetrain.rearLeftMotor)
self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(
RobotMap.drivetrain.rearRightMotor)
self.frontLeftMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.leftMotors = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.rightMotors = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors)
self.lastMoveValue = 0
self.lastRotateValue = 0
self.gyro = wpilib.ADXRS450_Gyro()
self.setpoint = 0
self.angleAcc = 0
self.anglePreviousTime = time.time()
self.rangeFinder = wpilib.AnalogInput(0)
self.didResetRevos = True
self.startTime = 0
self.revoVelos = []
self.gyro.calibrate()
def __init__(self):
super().__init__()
# Initiate up robot drive
self.left_motor = wpilib.Spark(LEFT_PORT)
self.right_motor = wpilib.Spark(RIGHT_PORT)
self.drive = DifferentialDrive(self.left_motor, self.right_motor)
self.drive.setExpiration(.1)
# Initiate arm and lift
self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT)
self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2)
# Initiate button stuff
self.buttons = set()
self.button_toggles = None
self.pressed_buttons = {}
self.rumble_time = None
self.rumble_length = RUMBLE_LENGTH
self.reset_buttons()
# Initiate gamepad
self.game_pad = wpilib.Joystick(GAMEPAD_NUM)
self.max_enhancer = 0
self.smart_dashboard = wpilib.SmartDashboard
# auto_chooser = wpilib.SendableChooser()
# auto_chooser.addDefault("Enable Auto", "Enable Auto")
# auto_chooser.addObject("Disable Auto", "Disable Auto")
self.smart_dashboard.putBoolean("Enable Auto", True)
self.auto_start_time = None
self.current_speed = [0, 0]
self.last_tank = 0
self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel
self.tank_dir = None
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
# camera
# utrasoni sensors
# motors
self.robot = self
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.left_motor.setInverted(True)
self.right_motor.setInverted(True)
self.lifter_motor = wpilib.Talon(2)
self.ultrasonic = wpilib.AnalogInput(0)
self.light_relay = wpilib.Relay(0)
self.light_relay.set(wpilib.Relay.Value.kOn)
self.myRobot = wpilib.RobotDrive(self.left_motor, self.right_motor)
self.myRobot.setSafetyEnabled(False)
#2Joysticks
self.leftStick = wpilib.Joystick(0)
#self.rightStick = wpilib.Joystick(1)
# 5 motor controlors: 1colocter, 2 for weels, 1 for shooter
#light
#lifter: 1 motor
#
self.gyro = wpilib.ADXRS450_Gyro()
wpilib.CameraServer.launch('vision.py:main')
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 robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#This initializes Networktables which will be used for communication between our robot and the GUI.
self.sd = NetworkTables.getTable('SmartDashboard')
#This initializes the camera.
wpilib.CameraServer.launch()
#This sets some counters for the state machines. (FOR Previous version without the encoders)
self.getCubeCounter = 0
self.dropCubeCounter = 0
self.elevatorDownCounter = 0
self.elevatorUpCounter = 0
self.cubeTravelUp = 50
self.cubeTravelStop = 1
#This sets some flags for the state machines.
self.prepareCubeFlag = 0 #This is the flag for the state machine which makes the robot prepared to grab the cube.
self.grabCubeFlag = 0 #This is the flag for the state machine which makes the robot grab the cube.
self.deliverCubeFlag = 0 #This is the flag for the state machine which makes the robot deliver the cube.
self.state = 0
#This sets the Drive Factor, which adjusts controller responsiveness.
self.driveFactor = 0.5
#This initializes the Encoders - left and right, attached to gearbox
self.EC1 = wpilib.Encoder(4, 5)
self.EC2 = wpilib.Encoder(6, 7)
#Encoder for the elevator and shoulder
self.EC3 = wpilib.Encoder(6,
7) #This sets the encoder for the elevator.
self.EC4 = wpilib.Encoder(8, 9) #This sets the encoder for the arm.
#This sets the Pneumatics.
self.leftGearShift = wpilib.Solenoid(5, 0)
self.rightGearShift = wpilib.Solenoid(5, 1)
self.goldenArrowhead = wpilib.Solenoid(5,
2) # Reference to Guyanese flag
#This controls the function of the arm.
# Include limit switches for the elevator and shoulder mechanisms
# 2018-2-16 Warning! The Switch's channel should be modified according to the robot! - Fixed
self.SW0 = wpilib.DigitalInput(0) #Lower Elevator Switch
self.SW1 = wpilib.DigitalInput(1) #Upper Elevator Switch
#self.SW2 = wpilib.DigitalInput(2) #Lower shoulder switch
#self.SW3 = wpilib.DigitalInput(3) #Upper shoulder switch
# Left Motor Group Setup
self.M0 = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.M1 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.M0.setInverted(True) #This inverts the motors.
self.M1.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M0, self.M1)
# Right Motor Group Setup
self.M2 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.M3 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.M2, self.M3)
# Drive setup
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setMaxOutput(self.driveFactor)
# Misc Setting
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
# E = Elevator
self.E1 = wpilib.VictorSP(0) #This initializes the left elevator.
self.E2 = wpilib.VictorSP(1) #This initializes the right elevator.
# Shoulder
self.S1 = wpilib.VictorSP(2) #This initializes the left shoulder.
self.S2 = wpilib.VictorSP(3) #This initializes the right shoulder.
#Servo
self.SV1 = wpilib.Servo(4) #This initializes a servo.
#self.SV2 = wpilib.Servo(5)
#self.SV1.set(0.0)
#self.SV2.set(0.0)
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(
0) #This initializes a gyro to detect the direction of the robot.
self.gyro.reset() #This resets the gyro.
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
"""
Button Map for Dual Joysticks
Right Joystick (Intake):
1. Chute + Loader (100%)
2. Climb Up
3. Chute + Loader (50%)
4. Loader (50%)
5. Chute (50%)
Left Joystick (Outtake):
1. Chute + Loader (-100%)
2. Climb Down
3. Chute + Loader (-50%)
4. Loader (-50%)
5. Chute (-50%)
"""
#Here is the encoder setup for the 4 motor drivetrain
self.l_motorFront = ctre.wpi_talonsrx.WPI_TalonSRX(0)
self.l_motorFront.setInverted(False)
self.l_motorBack = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.l_motorBack.setInverted(False)
self.r_motorFront = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.r_motorFront.setInverted(False)
self.r_motorBack = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.r_motorBack.setInverted(False)
self.l_motorFront.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.l_motorBack.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.r_motorFront.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.r_motorBack.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
## self.l_motorFront.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
## self.r_motorFront.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motorFront.setQuadraturePosition(0, 0)
self.r_motorFront.setQuadraturePosition(0, 0)
#Here is the encoder setup for the left and right chute motors
self.l_chute = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.l_chute.setInverted(False)
self.r_chute = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.r_chute.setInverted(False)
self.l_chute.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.r_chute.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
## self.l_chute.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
## self.r_chute.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
## self.l_chute.setQuadraturePosition(0, 0)
## self.r_chute.setQuadraturePosition(0, 0)
#This is the setup for the drive groups and loaders
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
self.l_loader = wpilib.Spark(0)
self.l_loader.setInverted(False)
self.r_loader = wpilib.Spark(1)
self.r_loader.setInverted(True)
self.climb = wpilib.Spark(2)
self.left = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack)
self.right = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.auto_switch0 = wpilib.DigitalInput(0)
self.auto_switch1 = wpilib.DigitalInput(1)
self.auto_switch2 = wpilib.DigitalInput(2)
self.auto_switch3 = wpilib.DigitalInput(3)
self.optical = wpilib.DigitalInput(4)
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.calibrate()
self.gyro.reset()
self.counter = 0
def createObjects(self):
# GOTTA DO STUFF
self.fork_switch = wpilib.DigitalInput(8)
self.gyro = wpilib.ADXRS450_Gyro()
self.left_motor = ctre.WPI_TalonSRX(5)
self.right_motor = ctre.WPI_TalonSRX(6)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.drive_stick = wpilib.Joystick(0)
self.forklift_stick = wpilib.Joystick(1)
# Other motors
self.winch_motor = ctre.WPI_TalonSRX(7)
self.arm_motor = ctre.WPI_TalonSRX(8)
# choose the sensor and sensor direction
self.winch_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx, self.kTimeoutMs)
# choose to ensure sensor is positive when output is positive
self.winch_motor.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.winch_motor.setInverted(False)
# set the peak and nominal outputs, 12V means full
self.winch_motor.configNominalOutputForward(0, self.kTimeoutMs)
self.winch_motor.configNominalOutputReverse(0, self.kTimeoutMs)
self.winch_motor.configPeakOutputForward(1, self.kTimeoutMs)
self.winch_motor.configPeakOutputReverse(-1, self.kTimeoutMs)
# Set the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.winch_motor.configAllowableClosedloopError(
0, self.kPIDLoopIdx, self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.winch_motor.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.winch_motor.config_kF(0, 0, self.kTimeoutMs)
self.winch_motor.config_kP(0, 0.9, self.kTimeoutMs)
self.winch_motor.config_kI(0, 0, self.kTimeoutMs)
self.winch_motor.config_kD(0, 0, self.kTimeoutMs)
# zero the sensor
self.winch_motor.setSelectedSensorPosition(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# Elevator motor/sensors
#self.elevator_top = wpilib.DigitalInput(1)
#self.elevator_mid = wpilib.DigitalInput(2)
#self.elevator_bottom = wpilib.DigitalInput(3)
#Drive motors
self.joy = wpilib.Joystick(0)
wpilib.CameraServer.launch()
def create_adxrs450_gyro(port=None):
return wpilib.ADXRS450_Gyro(port)
def robotInit(self):
# Gear plate limit switch
self.gear_switch = wpilib.DigitalInput(const.ID_GEAR_SWITCH)
### Subsystems ###
# Agitator
self.agitator = subsystems.agitator.Agitator(self)
# Climber
self.climber = subsystems.climber.Climber(self)
# Drive for Xbox Controller
self.drive = subsystems.drivetrain.Drivetrain(self)
# Gear
self.gear_funnel = subsystems.gear_funnel.Gear_Funnel(self)
self.gear_lexan = subsystems.gear_lexan.Gear_Lexan(self)
self.gear_claw = subsystems.gear_claw.Gear_Claw(self)
# Ground Feeder
self.feeder = subsystems.feeder.Feeder(self)
# Shooter
self.shooter = subsystems.shooter.Shooter(self)
# Shooter Camera
self.shooter_camera = subsystems.shooter_camera.Shooter_Camera(self)
# Gear Camera
self.gear_camera = subsystems.gear_camera.Gear_Camera(self)
# Encoders
self.drive_encoder_left = wpilib.Encoder(2, 3, reverseDirection=True)
self.drive_encoder_left.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_left.reset()
self.drive_encoder_right = wpilib.Encoder(0, 1, reverseDirection=False)
self.drive_encoder_right.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_right.reset()
# Pressure sensor (200 psi)
self.pressure_sensor = wpilib.AnalogInput(const.ID_PRESSURE_SENSOR)
self._pressure_samples = []
self._last_pressure_value = 0.0
# Gyro
self.gyro = wpilib.ADXRS450_Gyro()
#self.gyro = wpilib.AnalogGyro( 0 )
#self.gyro.setSensitivity( 0.08 )
### Misc ###
# Operator Input
self.oi = oi.OI(self)
# Log to file
self.log_file = open(
os.path.join(os.path.dirname(__file__), 'log.csv'), 'w')
# Time robot object was created
self.start_time = time.time()
## Autonomous ##
self.subsystems = {
'agitator': self.agitator,
'climber': self.climber,
'drive': self.drive,
'gear_funnel': self.gear_funnel,
'gear_lexan': self.gear_lexan,
'gear_claw': self.gear_claw,
'feeder': self.feeder,
'shooter_camera': self.shooter_camera,
'gear_camera': self.gear_camera,
'shooter': self.shooter,
}
## Scheduler ##
self.scheduler = wpilib.command.Scheduler.getInstance()
## MISC ##
self.gear_is_ejecting = False
self.gear_ejecting_start_time = 0
### Logging ###
# NetworkTables
self.nt_smartdash = networktables.NetworkTable.getTable(
'SmartDashboard')
self.nt_grip_peg = networktables.NetworkTable.getTable(
'vision/peg_targets')
self.nt_grip_boiler = networktables.NetworkTable.getTable(
'vision/boiler_targets')
self.nt_vision = networktables.NetworkTable.getTable('vision')
# Timers for NetworkTables update so we don't use too much bandwidth
self.log_timer = wpilib.Timer()
self.log_timer.start()
self.log_timer_delay = 0.1 # 10 times/second
# Timer for pressure sensor's running average
self.pressure_timer = wpilib.Timer()
self.pressure_timer.start()
self.pressure_timer_delay = 1.0 # once per second
self.log()
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = wp.VictorSP(1) #motor initialization
self.leftMotor2 = wp.VictorSP(3)
self.leftMotor3 = wp.VictorSP(5)
self.rightMotor1 = wp.VictorSP(2)
self.rightMotor2 = wp.VictorSP(4)
self.rightMotor3 = wp.VictorSP(6)
self.armMotor = wp.VictorSP(7)
self.leftIntakeMotor = wp.VictorSP(8)
self.rightIntakeMotor = wp.VictorSP(9)
self.leftSide = wp.SpeedControllerGroup(
self.leftMotor1, self.leftMotor2,
self.leftMotor3) #speed controller groups
self.rightSide = wp.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2,
self.rightMotor3)
self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor,
self.rightIntakeMotor)
self.myRobot = DifferentialDrive(self.leftSide, self.rightSide)
self.myRobot.setExpiration(0.1)
self.leftStick = wp.Joystick(0)
self.rightStick = wp.Joystick(1)
self.armStick = wp.XboxController(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(2, 3)
self.leftEncd = wp.Encoder(0, 1)
self.armPot = wp.AnalogPotentiometer(0, 270, -135)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
wp.SmartDashboard.putNumber("Straight Position", 15000)
wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000)
wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000)
wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000)
wp.SmartDashboard.putNumber("Left Switch Angle", 90)
wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000)
wp.SmartDashboard.putNumber("Switch Score Arm Position", 70)
wp.SmartDashboard.putNumber("Front Position 1", 74)
wp.SmartDashboard.putNumber("Front Position 2", 16)
wp.SmartDashboard.putNumber("Front Position 3", -63)
wp.SmartDashboard.putNumber("lvl2 Position 1", 60)
wp.SmartDashboard.putNumber("lvl2 Position 3", -50)
wp.SmartDashboard.putNumber("lvl3 Position 3", -38)
wp.SmartDashboard.putNumber("lvl3 Position 1", 45)
wp.SmartDashboard.putBoolean("switchScore?", False)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Side Switch Auto", 2)
self.chooser.addObject("Right Side Switch Auto (switch)", 3)
self.chooser.addObject("Center Auto", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
"""
1. Chute
2. Loader
3. Climber
"""
# Configure shooter motor controller.
self.Gyro = wpilib.ADXRS450_Gyro()
self.Chute = ctre.wpi_talonsrx.WPI_TalonSRX(7)
self.Chute = ctre.wpi_talonsrx.WPI_TalonSRX(
8) # Create a CANTalon object.
self.Chute.configSelectedFeedbackSensor(
ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0
) # Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure.
# I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12.
# Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM.
#self.shooter.configEncoderCodesPerRev(48)
# resets shooter position on startup
#self.unloader.setQuadraturePosition(0, 0)
#self.unloader.setNeutralMode(ctre.wpilib.Spark.NeutralMode.Coast)# This should change between brake and coast modes.
self.l_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(0)
self.l_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.l_motor1.setInverted(False)
self.l_motor2.setInverted(False)
self.r_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.r_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.r_motor1.setInverted(False)
self.r_motor2.setInverted(False)
# Configure shooter motor controller.
# Create a CANTalon object.
self.l_motor1.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.l_motor1.configSelectedFeedbackSensor(
ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motor2.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.l_motor2.configSelectedFeedbackSensor(
ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motor1.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.r_motor1.configSelectedFeedbackSensor(
ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motor2.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.r_motor2.configSelectedFeedbackSensor(
ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0
) # Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure.
# I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12.
# Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM.
#self.l_motor1.configEncoderCodesPerRev(48)
#self.l_motor2.configEncoderCodesPerRev(48)
#self.r_motor1.configEncoderCodesPerRev(48)
#self.r_motor2.configEncoderCodesPerRev(48)
# resets shooter position on startup
self.l_motor1.setQuadraturePosition(0, 0)
self.l_motor2.setQuadraturePosition(0, 0)
self.r_motor1.setQuadraturePosition(0, 0)
self.r_motor2.setQuadraturePosition(0, 0)
#self.stick = wpilib.Joystick(0)
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
self.loader = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.climber = wpilib.Spark(6)
self.loader = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.drive = wpilib.RobotDrive(self.l_motor1, self.l_motor2,
self.r_motor1, self.r_motor2)
self.counter = 0
self.mode = 0
