def robotInit(self):
# Robot initialization function
# VictorSPX = Motor Controllers
self.frontLeftMotor = ctre.WPI_VictorSPX(0)
self.rearLeftMotor = ctre.WPI_VictorSPX(1)
self.frontRightMotor = ctre.WPI_VictorSPX(4)
self.rearRightMotor = ctre.WPI_VictorSPX(5)
self.basketMotor = ctre.WPI_VictorSPX(3)
# Digital Inputs (Limit Switch)
self.basketLimitSwitch = wpilib.DigitalInput(0)
# Motor controller groups for each side of the robot
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# Differential drive with left and right motor controller groups
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.direction = -1
# Declare gamepad
self.gamepad = wpilib.Joystick(0)
# Declare buttons
# Controller mapping (1-10): B (East), A (South), Y (North), X (West), Right Bumper, Left Bumper, ?, ?, ?, ?
self.toggleHatchButton = JoystickButton(self.gamepad, 1)
self.toggleBasketButton = JoystickButton(self.gamepad, 2)
self.toggleDirectionButton = JoystickButton(self.gamepad, 3)
self.speedUpButton = JoystickButton(self.gamepad, 4)
self.raiseBasketButton = JoystickButton(self.gamepad, 5)
self.lowerBasketButton = JoystickButton(self.gamepad, 6)
# Determine if already acted on
self.hatchActed = False
self.basketActed = False
self.directionActed = False
# Solenoids
self.hatchSolenoid = wpilib.DoubleSolenoid(0, 1)
self.basketSolenoid = wpilib.DoubleSolenoid(2, 3)
# Compressor
self.compressor = wpilib.Compressor(0)
# Camera Server
wpilib.CameraServer.launch()
def robotInit(self):
super().__init__()
# gc.collect()
# #CommandBasedRobot.__init__()
# # Instances of classes
#
# # Instantiate Subsystems
# # self.shifters = Shifters()
self.drivetrain = Drivetrain(self)
# gc.collect()
# self.shooter = Shooter(self)
# self.carrier = Carrier(self)
# self.feeder = Feeder(self)
# self.intake = Intake(self)
# self.winch = Winch(self)
# self.climber = Climber(self)
#
# # Instantiate Joysticks
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
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)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
def module_init(self):
# Setup a device references
self.joystick = wpilib.Joystick(0)
self.left_front_motor = wpilib.CANTalon(1)
self.left_front_motor.enableControl()
self.left_rear_motor = wpilib.CANTalon(2)
self.left_rear_motor.enableControl()
self.right_front_motor = wpilib.CANTalon(3)
self.right_front_motor.enableControl()
self.right_rear_motor = wpilib.CANTalon(4)
self.right_rear_motor.enableControl()
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.AnalogGyro(1)
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 robotInit(self):
self.gamepad = wpilib.Joystick(0)
self.flywheelMotor = CANTalon(5)
self.speed = 20000
self.flywheelMotor.setFeedbackDevice(
CANTalon.FeedbackDevice.QuadEncoder)
self.inSpeedMode = False
self.flywheelMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick)
self.intake = Intake(self.intake_wheels_motor, self.intake_arm_motor,
self.arm_stick)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick)
self.shooter = Shooter(self.shooter_motor, self.encoder,
self.shooter_servo, self.arm_stick,
self.shooter_service)
self.autonomous = Autonomous(self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor)
def robotInit(self):
self.frontRight = wpilib.Talon(0)
self.rearRight = wpilib.Talon(1)
self.frontRight.setInverted(True)
self.rearRight.setInverted(True)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.frontLeft = wpilib.Talon(2)
self.rearLeft = wpilib.Talon(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
self.leftEncoder = wpilib.Encoder(8, 9)
self.leftEncoder.setDistancePerPulse(1 / 2 * math.pi / 256)
self.leftEncoder.setSamplesToAverage(10)
self.table = NetworkTables.getTable('limelight')
def createObjects(self):
# Inputs
# TODO: Update these dynamically
self.stick = wpilib.Joystick(2)
# self.gamepad = wpilib.XboxController(1)
# self.gamepad_alt = wpilib.XboxController(2)
self.gamepad = wpilib.XboxController(0)
self.gamepad_alt = wpilib.XboxController(1)
# Drive motors
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive_train = wpilib.drive.DifferentialDrive(
self.left_motor, self.right_motor)
# Elevator encoder (gearbox)
self.lift_encoder = ExternalEncoder(0, 1)
# TODO: Fix the pid
# self.lift_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Drive encoders
self.left_encoder = ExternalEncoder(2, 3)
self.right_encoder = ExternalEncoder(4, 5, True)
self.left_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
self.right_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Elevator motors
self.lift_master = CANTalon(2)
self.lift_follower1 = CANTalon(3)
self.lift_follower2 = CANTalon(4)
self.lift_follower1.follow(self.lift_master)
self.lift_follower2.follow(self.lift_master)
# Intake motors
self.left_intake_motor = wpilib.Spark(2)
self.right_intake_motor = wpilib.Spark(3)
self.right_intake_motor.setInverted(True)
# Intake grabbers
self.grabber_solenoid = wpilib.DoubleSolenoid(1, 0, 1)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
wpilib.SmartDashboard.putData("PowerDistributionPanel", self.pdp)
# Misc
self.navx = navx.AHRS.create_spi()
self.net_table = NetworkTables.getTable("SmartDashboard")
# Launch camera server
wpilib.CameraServer.launch()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.left_motor = wpilib.VictorSP(0)
self.right_motor = wpilib.VictorSP(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.drive.setRightSideInverted(True)
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
def robotInit(self):
self.joystick = wpilib.Joystick(0)
self.driveButton = wpilib.buttons.JoystickButton(self.joystick, 2)
self.lMotor = ctre.WPI_VictorSPX(1)
self.rMotor = ctre.WPI_VictorSPX(2)
self.driveTrain = wpilib.drive.DifferentialDrive(
self.lMotor, self.rMotor)
self.driveTrain.setRightSideInverted(False)
self.tankDrive = False
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.motor1 = WPI_TalonSRX(1)
self.motor2 = WPI_TalonSRX(2)
self.drive = wpilib.drive.DifferentialDrive(self.motor1, self.motor2)
# joysticks 1 & 2 on the driver station
#self.motor1 = WPI_TalonSRX(2)
self.stick = wpilib.Joystick(0)
def robotInit(self):
'''Robot initialization function'''
self.map = RobotMap()
self.createMotors(self.map.CAN.driveMotors)
#inti the drive train
driveTrain.init(self.driveMotors)
subsystems.init()
self.autonomousProgram = autonomous.AutonomousProgram()
self.driveStick = wpilib.Joystick(
self.map.joystickMap.movementJoystick)
def robotInit(self):
# Create motors
self.lf_motor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.lb_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rf_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.rb_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
# You must call getPIDController() on an existing CANSparkMax or
# SparkMax object to fully use PID functionality
# self.l_pidController = self.lf_motor.getPIDController()
# self.r_pidController = self.rf_motor.getPIDController()
# Instantiate built-in encoder to display position
# self.l_encoder = self.lf_motor.getEncoder()
# self.r_encoder = self.rf_motor.getEncoder()
self.joystick = wpilib.Joystick(0)
# PID Coefficents and Controller Output Range
self.coeff = {"p": 0.22, "i": 1.188, "d": 0, "iz": 0, "ff": 0}
self.kMinOutput = -0.5
self.kMaxOutput = 0.5
# Motor max RPM
self.maxRPM = 5700
# The restoreFactoryDefaults() method can be used to reset the
# configuration parameters in the SPARK MAX to their factory default
# state. If no argument is passed, these parameters will not persist
# between power cycles
# Set PID Coefficents
# self.l_pidController.setP(self.coeff["p"])
# self.l_pidController.setI(self.coeff["i"])
# self.l_pidController.setD(self.coeff["d"])
# self.l_pidController.setIZone(self.coeff["iz"])
# self.l_pidController.setFF(self.coeff["ff"])
# self.l_pidController.setOutputRange(self.kMinOutput, self.kMaxOutput)
# self.r_pidController.setP(self.coeff["p"])
# self.r_pidController.setI(self.coeff["i"])
# self.r_pidController.setD(self.coeff["d"])
# self.r_pidController.setIZone(self.coeff["iz"])
# self.r_pidController.setFF(self.coeff["ff"])
# self.r_pidController.setOutputRange(self.kMinOutput, self.kMaxOutput)
# Push PID Coefficients to SmartDashboard
wpilib.SmartDashboard.putNumber("P Gain", self.coeff["p"])
wpilib.SmartDashboard.putNumber("I Gain", self.coeff["i"])
wpilib.SmartDashboard.putNumber("D Gain", self.coeff["d"])
wpilib.SmartDashboard.putNumber("I Zone", self.coeff["iz"])
wpilib.SmartDashboard.putNumber("Feed Forward", self.coeff["ff"])
wpilib.SmartDashboard.putNumber("Min Output", self.kMinOutput)
wpilib.SmartDashboard.putNumber("Max Output", self.kMaxOutput)
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.stick = wpilib.Joystick(0)
# TODO: change to correct motors
self.intake_motor = wpilib.PWMTalonSRX(1)
self.belt_motor = wpilib.PWMSparkMax(2)
self.ball_sensor1 = wpilib.AnalogInput(1)
self.ball_sensor2 = wpilib.AnalogInput(2)
self.ball_sensor3 = wpilib.AnalogInput(3)
def robotInit(self):
"""Robot initialization function"""
# create motor controller objects
m_left = wpilib.Talon(0)
m_right = wpilib.Talon(1)
# object that handles basic drive operations
self.myRobot = wpilib.drive.DifferentialDrive(m_left, m_right)
self.myRobot.setExpiration(0.1)
# joystick #0
self.stick = wpilib.Joystick(0)
def robotInit(self):
self.BRmotor = ctre.wpi_talonsrx.WPI_TalonSRX(40)
self.BLmotor = ctre.wpi_talonsrx.WPI_TalonSRX(50)
self.FRmotor = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.FLmotor = ctre.wpi_talonsrx.WPI_TalonSRX(30)
self.servo = wpilib.Servo(8)
self.timer = wpilib.Timer()
self.robot_drive = wpilib.RobotDrive(self.FLmotor, self.BLmotor, self.FRmotor, self.BRmotor)
self.joystick = wpilib.Joystick(0)
def robotInit(self):
self.left_slave_motor = ctre.WPI_TalonSRX(LEFT_SLAVE_MOTOR_ID)
self.left_master_motor = ctre.WPI_TalonSRX(LEFT_MASTER_MOTOR_ID)
self.right_slave_motor = ctre.WPI_TalonSRX(RIGHT_SLAVE_MOTOR_ID)
self.right_master_motor = ctre.WPI_TalonSRX(RIGHT_MASTER_MOTOR_ID)
self.left_slave_motor.follow(self.left_master_motor)
self.right_slave_motor.follow(self.right_master_motor)
self.drive = wpilib.drive.DifferentialDrive(self.left_master_motor,
self.right_master_motor)
self.stick = wpilib.Joystick(MAIN_JOYSTICK_ID)
def robotInit(self): #This is a function or method
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.RobotDrive(5, 6)
self.stick = wpilib.Joystick(0)
self.motor1 = ctre.WPI_TalonSRX(
1) # Initialize the TalonSRX on device 1.
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
def setUp(self):
print('setUp()')
with patch('drivetrain.Drivetrain') as mock:
dt_inst = mock.return_value
self.drive_inst = dt_inst.drive
# self.left_motors = Left_Motors()
# self.right_motors = Right_Motors()
#
# self.dt = Drivetrain(robot_instance)
#
# def test_set_drivetrain_type(self):
# print('test_set_drivetrain_type()')
# assert(self.param.set_drivetrain_type(
# wpilib.drive.DifferentialDrive,
# self.robot_instance.left_motors, self.robot_instance.right_motors\
# == wpilib.drive.DifferentialDrive(
# self.left_motors, self.right_motors)
# ))
self.left_joy = wpilib.Joystick(0)
self.right_joy = wpilib.Joystick(1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#self.left_motor = wpilib.Spark(0)
#self.right_motor = wpilib.Spark(1)
#self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
self.motor = wpilib.Talon(0)
def robotInit(self):
self.stick1 = wpilib.Joystick(0)
self.stick2 = wpilib.Joystick(1)
self.rightTop = ctre.WPI_VictorSPX(21)
self.rightMiddle = ctre.WPI_TalonSRX(20)
self.rightBottom = ctre.WPI_VictorSPX(22)
self.drive_right = wpilib.drive.SpeedControllerGroup(
rightTop, rightMiddle, rightBottom)
self.leftTop = ctre.WPI_VictorSPX(11)
self.leftMiddle = ctre.WPI_TalonSRX(10)
self.leftBottom = ctre.WPI_VictorSPX(12)
self.drive_left = wpilib.drive.SpeedControllerGroup(
leftTop, leftMiddle, leftBottom)
self.drive = wpilib.DifferentialDrive(self.drive_right,
self.drive_left)
def teleopPeriodic(self):
self.drive.tankDrive(self.stick1.getY(), self.stick.getX())
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
# TODO: create button example here
# self.component1_motor = wpilib.Talon(1)
# self.some_motor = wpilib.Talon(2)
self.lMotor = wpilib.Talon(1)
self.rMotor = wpilib.Talon(2)
self.joystick = wpilib.Joystick(0)
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
self.motor_D1 = wpilib.Victorsp(1)
self.motor_I1 = wpilib.Victorsp(2)
self.G_motor = wpilib.SpeedControllerGroup(self.motor_D1,
self.motor_I1)
self.robotdrive = wpilib.drive.differentialdrive(
self.motor_D1, self.motor_I1)
def init_drive_train(self):
fl, bl, fr, br = (30, 40, 50, 10) # practice bot
br, fr, bl, fl = (1, 7, 2, 5) # on competition robot
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(br)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(bl)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(fl)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(fr)
self.fr_motor.setInverted(True)
self.br_motor.setInverted(True)
self.gyro = navx.AHRS.create_spi()
self.joystick = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.bl_motor,
self.fr_motor, self.br_motor)
self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.bl_motor,
self.fr_motor, self.br_motor)
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
"a",
steer_talon=ctre.WPI_TalonSRX(48),
drive_talon=ctre.WPI_TalonSRX(49),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.WPI_TalonSRX(46),
drive_talon=ctre.WPI_TalonSRX(47),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.WPI_TalonSRX(44),
drive_talon=ctre.WPI_TalonSRX(45),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.WPI_TalonSRX(42),
drive_talon=ctre.WPI_TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.WPI_TalonSRX(14)
self.intake_right_motor = ctre.WPI_TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(0)
self.intake_kicker = wpilib.Solenoid(1)
self.extension_arms = wpilib.Solenoid(3)
self.infrared = SharpIRGP2Y0A41SK0F(0)
self.lifter_motor = ctre.WPI_TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
# create the imu object
self.imu = NavX()
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 5
self.sd = NetworkTables.getTable("SmartDashboard")
def createObjects(self):
drive = Drive
lift = Lift
#collection = Collection
self.compressor = wpilib.Compressor()
#Establishing Dashboard
self.HUD = wpilib.SmartDashboard
self.limitSwitchIn = wpilib.DigitalInput(0)
#Left motors
self.motor_l1 = ctre.WPI_TalonSRX(8)
self.motor_l2 = ctre.WPI_TalonSRX(9)
self.motor_l3 = ctre.WPI_TalonSRX(10)
#Right motors
self.motor_r1 = ctre.WPI_TalonSRX(2)
self.motor_r2 = ctre.WPI_TalonSRX(3)
self.motor_r3 = ctre.WPI_TalonSRX(4)
#Lift motors
self.lift_motor1 = ctre.WPI_TalonSRX(1)
self.lift_motor2 = ctre.WPI_TalonSRX(7)
#Collection motors
self.collection_motor = ctre.WPI_TalonSRX(11)
self.climbing_motor1 = ctre.WPI_TalonSRX(5)
self.climbing_motor2 = ctre.WPI_TalonSRX(12)
#DoubleSolenoids
self.doubleS = wpilib.DoubleSolenoid(0, 1)
self.doubleS2 = wpilib.DoubleSolenoid(2, 3)
#Controllers
self.stick = wpilib.Joystick(0)
self.stick2 = wpilib.Joystick(1)
def createObjects(self):
# Define Driving Motors
self.rightDrive = wpilib.VictorSP(0)
self.leftDrive = wpilib.VictorSP(1)
# Create Robot Drive
self.robotDrive = wpilib.drive.DifferentialDrive(self.rightDrive, self.leftDrive)
# Create Shifter Pneumatics
self.shifterSolenoid = wpilib.DoubleSolenoid(0, 0, 1)
# Joysticks and Controllers
self.driveJoystick = wpilib.Joystick(0)
self.driveController = XboxController(0)
self.subsystemJoystick = wpilib.Joystick(1)
self.subsystemController = XboxController(1)
# Create NavX and Accel
self.navX = navx.AHRS.create_spi()
self.accel = wpilib.BuiltInAccelerometer()
# Set Drivespeed
self.driveSpeed = 0
# Create Cube Intake Pneumatics
self.intakeSolenoid = wpilib.Solenoid(0, 2)
# Create Timer (For Making Timed Events)
self.timer = wpilib.Timer()
# Initialize Compressor
self.compressor = wpilib.Compressor()
# Create CameraServer
wpilib.CameraServer.launch("common/multipleCameras.py:main")
# Set Gear in Dashboard
wpilib.SmartDashboard.putString("Shift State", "Low Gear")
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
# setup camera
# setup master and slave drive motors
self.drive_slave_left = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_LEFT_ID)
self.drive_master_left = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_LEFT_ID)
self.drive_master_left.follow(self.drive_slave_left)
self.drive_slave_right = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_RIGHT_ID)
self.drive_master_right = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_RIGHT_ID)
self.drive_master_right.follow(self.drive_slave_right)
# setup actuator motors
self.intake_motor = lazytalonsrx.LazyTalonSRX(self.INTAKE_ID)
self.low_tower_motor = lazytalonsrx.LazyTalonSRX(self.LOW_TOWER_ID)
self.high_tower_motor = lazytalonsrx.LazyTalonSRX(self.HIGH_TOWER_ID)
self.turret_motor = lazytalonsrx.LazyTalonSRX(self.TURRET_ID)
self.turret_motor.setEncoderConfig(lazytalonsrx.CTREMag, True)
self.flywheel_motor = rev.CANSparkMax(self.FLYWHEEL_MOTOR_ID,
rev.MotorType.kBrushless)
self.climb_winch_slave = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_SLAVE_ID)
self.climb_winch_master = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_MASTER_ID)
self.climb_winch_slave.follow(self.climb_winch_master)
self.slider_motor = lazytalonsrx.LazyTalonSRX(self.SLIDER_ID)
# setup imu
self.imu = lazypigeonimu.LazyPigeonIMU(self.intake_motor)
# setup proximity sensors
self.tower_sensors = [wpilib.DigitalInput(i) for i in range(0, 5)]
self.intake_sensors = [wpilib.DigitalInput(8), wpilib.DigitalInput(9)]
# setup joysticks
self.driver = wpilib.Joystick(0)
self.operator = wpilib.XboxController(1)
self.nt = NetworkTables.getTable("robot")
self.manual_indexer = True
self.chassis_slow = False
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.RobotDrive(0,1,2,3)
self.stick = wpilib.Joystick(0)
self.Motor1 = wpilib.VictorSP(4)
self.Motor2 = wpilib.VictorSP(5)
self.Switch1 = wpilib.DigitalInput(0)
self.Switch2 = wpilib.DigitalInput(1)
self.Servo1 = wpilib.Servo(6)
self.Servo2 = wpilib.Servo(7)
