def robotInit(self):
# Motor Init
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
# Arm Init
self.arm = ctre.WPI_TalonSRX(5)
# Speed control groups
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
# Drive Function Init
self.driveMeBoi = wpilib.drive.DifferentialDrive(self.left, self.right)
#Controller Init
self.controller = wpilib.XboxController(0)
# Sensor
self.intakeSensor = wpilib.DigitalInput(9)
#Color.py Init
self.color = color.PrintColor()
#Auto mode variables
self.components = {'drive': self.driveMeBoi}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.drive = drive.Drive(self.driveMeBoi)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.timer = wpilib.Timer()
self.gyro = ADXLGyro.ADXLGyro()
self.leftGearbox = Gearbox.Gearbox([0, 1, 2])
self.rightGearbox = Gearbox.Gearbox([3, 4, 5], inverted=True)
self.intake = Intake.Intake()
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.prefs = wpilib.Preferences.getInstance()
self.prefs.put("Robot", "CraigPy")
self.components = {
'left': self.leftGearbox,
'right': self.rightGearbox,
'intake': self.intake,
'gyro': self.gyro,
'prefs': self.prefs,
'isSim': self.isSimulation(),
'utils': Utils
}
self.autonomous = AutonomousModeSelector('Autonomous', self.components)
self.gyro.calibrate()
self.i = 0
def robotInit(self):
#Motors
self.leftMotorInput = wpilib.Talon(1) # AEN
self.rightMotorInput = wpilib.Talon(2) # AEN
self.drive = wpilib.drive.DifferentialDrive(self.leftMotorInput, self.rightMotorInput)
#Inputs
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
#Navigation and Logistics
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ["logger", "members"]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector("autonomous")
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable("/robot")
self.__nt.putBoolean("is_simulation", self.isSimulation())
self.__nt.putBoolean("is_ds_attached", self.ds.isDSAttached())
def robotInit(self):
#Drive Motors
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)
self.motor5 = ctre.WPI_TalonFX(5) #intake Motor
self.motor6 = ctre.WPI_TalonFX(6) #Shooter Motor
self.motor7 = ctre.WPI_VictorSPX(7) #Intake Arm
self.motor8 = ctre.WPI_VictorSPX(8) #Belt Drive
self.joy = wpilib.Joystick(0)
self.stick = wpilib.Joystick(
1) #this is a controller, also acceptable to use Joystick
self.intake = wpilib.DoubleSolenoid(0, 1)
self.balls = wpilib.DoubleSolenoid(2, 3)
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
#This is combining the Speed controls from above to make a left and right
#for the drive chassis. Note that self.motor1 and self.motor2 are combined to make self.left
#then self.motor3 and self.motor4 are combined to make self.right. This is done to Make
#the differential drive easier to setup
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
#Here is our DifferentialDrive, Ultimately stating, Left side and Right side of chassis
#An Alternative to DifferentialDrive is this:
#self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2)
#where motor 1 & 4 are left, and 2 & 3 are right
self.myRobot.setExpiration(0.1)
#These components here are for Autonomous Modes, and allows you to call parts and
#components here to be used IN automodes. For example- Our chassis above is labeled
#'self.myRobot', below in the self.components, If we want to use our chassis to drive
#in Autonomous, we need to call it in the fashion below. It is also encouraged to
#reuse the naming of the components from above to avoid confusion below. i.e.
#'Chassis': self.myRobot, In autonomous creation, I would then be using the variable
#self.chassis.set() instead of self.myRobot.set() when doing commands.
self.components = {
'myRobot': self.myRobot, #Chassis Driving
'motor5': self.motor5,
'motor6': self.motor6, #A speed control that is used for intake
'intake': self.intake,
'balls': self.balls #pneumatics arm that is not setup on bot yet
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
# Simple two wheel drive
self.drive = wpilib.RobotDrive(0, 1)
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {"drive": self.drive}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector("autonomous", self.components)
def robotInit(self):
wpilib.CameraServer.launch('misc/vision.py:main')
if not wpilib.RobotBase.isSimulation(
): #This makes simulator show motor outputs for debugging
import ctre
self.RLC = ctre.CANTalon(self.rLeftChannel)
self.FLC = ctre.CANTalon(self.fLeftChannel)
self.FRC = ctre.CANTalon(self.fRightChannel)
self.RRC = ctre.CANTalon(self.rRightChannel)
else:
self.RLC = wpilib.Talon(self.rLeftChannel)
self.FLC = wpilib.Talon(self.fLeftChannel)
self.FRC = wpilib.Talon(self.fRightChannel)
self.RRC = wpilib.Talon(self.rRightChannel)
self.robotDrive = wpilib.RobotDrive(
self.RLC, self.RRC, self.FRC,
self.FLC) #Sets motors for robotDrive commands
#Controller Input Variables
self.controller = wpilib.Joystick(self.joystickChannel)
self.winch_backward = wpilib.buttons.JoystickButton(self.controller, 5)
self.paddleGet = wpilib.buttons.JoystickButton(self.controller, 1)
self.gearDrop = wpilib.buttons.JoystickButton(self.controller,
6) # Right Bumper
self.xboxMec = wpilib.buttons.JoystickButton(self.controller, 8)
self.xboxMec2 = wpilib.buttons.JoystickButton(self.controller, 7)
#CRio Output Variables
self.winch_motor1 = wpilib.Talon(7)
self.winch_motor2 = wpilib.Talon(8)
self.paddle = wpilib.Solenoid(1)
self.gear = wpilib.DoubleSolenoid(2, 3)
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.navx = navx.AHRS.create_spi()
#Auto mode variables
self.components = {
'drive': self.robotDrive,
'gearDrop': self.gear,
'ultrasonic': self.ultrasonic,
'navx': self.navx
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
''' Robot initilization function '''
# initialize networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('shooter_speed', self.shooter_speed)
self.smart_dashboard.putNumber('gear_arm_opening_speed',
self.gear_arm_opening_speed)
self.smart_dashboard.putNumber('gear_arm_closing_speed',
self.gear_arm_closing_speed)
self.smart_dashboard.putNumber('loader_speed', self.loader_speed)
# initialize and launch the camera
wpilib.CameraServer.launch()
# object that handles basic drive operatives
self.drive_rf_motor = wpilib.Victor(portmap.motors.right_front)
self.drive_rr_motor = wpilib.Victor(portmap.motors.right_rear)
self.drive_lf_motor = wpilib.Victor(portmap.motors.left_front)
self.drive_lr_motor = wpilib.Victor(portmap.motors.left_rear)
self.shooter_motor = wpilib.Victor(portmap.motors.shooter)
self.gear_arm_motor = wpilib.Spark(portmap.motors.gear_arm)
self.loader_motor = wpilib.Spark(portmap.motors.loader)
# initialize drive
self.drive = wpilib.RobotDrive(self.drive_lf_motor,
self.drive_lr_motor,
self.drive_rf_motor,
self.drive_rr_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(1)
# initialize autonomous components
self.components = {
'drive': self.drive,
'gear_arm_motor': self.gear_arm_motor
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
#NetworkTables Init
NetworkTables.initialize()
self.table = NetworkTables.getTable("SmartDashboard")
#Navx
self.navx = navx.AHRS.create_spi()
#PowerDistributionPanel
self.power_board = wpilib.PowerDistributionPanel()
#Motors
self.leftMotor1 = wpilib.Spark(
0
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.leftMotor2 = wpilib.Spark(
1
) # <-- This is what links our PWM port on the CRIO to a physical ESC.
self.left = wpilib.SpeedControllerGroup(self.leftMotor1,
self.leftMotor2)
self.rightMotor1 = wpilib.Spark(2)
self.rightMotor2 = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2)
self.liftMotor1 = wpilib.Talon(4)
self.liftMotor2 = wpilib.Talon(5)
self.liftMotor2.setInverted(True)
self.lift = wpilib.SpeedControllerGroup(self.liftMotor1,
self.liftMotor2)
#User Input
self.playerOne = wpilib.XboxController(
0) # <-- This is for using Xbox controllers
#Drive
self.robotDrive = wpilib.drive.DifferentialDrive(self.left, self.right)
#Drive.py init
self.drive = drive.Drive(self.robotDrive, self.navx, self.left,
self.right)
self.components = {'drive': self.drive, 'table': self.table}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def robotInit(self):
self.chandler = XboxController(0)
self.meet = XboxController(1)
self.drive = drive.Drive() # So redundant omg
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer() # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
quickdebug.add_printables(self, ('match_time', Timer.getMatchTime))
quickdebug.init()
def robotInit(self):
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.ultrasonic = wpilib.AnalogInput(0)
self.distanceSensor = navx.AHRS.create_i2c()
self.playerOne = wpilib.XboxController(0)
self.robotDrive = wpilib.RobotDrive(self.motor1, self.motor2,
self.motor3, self.motor4)
self.components = {
'robotDrive': self.robotDrive,
'ultrasonic': self.ultrasonic
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.navx = navx.AHRS.create_spi()
def robotInit(self):
#Defining Constants
self.LeftTread = wpilib.Talon(0)
self.RightTread = wpilib.Talon(1)
self.robotDrive = wpilib.RobotDrive(self.LeftTread, self.RightTread)
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(self.xboxController, 4)
self.navx = navx.AHRS.create_spi()
self.drive = drive.Drive(self.robotDrive, self.xboxController, self.navx)
#Defining Variables
self.dm = True
#Auto mode variables
self.components = {
'drive': self.drive
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
"""
.. warning:: Internal API, don't override; use :meth:`createObjects` instead
"""
self._exclude_from_injection = ["logger"]
self.__last_error_report = -10
self._components = []
self._feedbacks = []
self._reset_components = []
# Create the user's objects and stuff here
self.createObjects()
# Load autonomous modes
self._automodes = AutonomousModeSelector("autonomous")
# Next, create the robot components and wire them together
self._create_components()
self.__nt = NetworkTables.getTable("/robot")
self.__nt_put_is_ds_attached = self.__nt.getEntry(
"is_ds_attached").setBoolean
self.__nt_put_mode = self.__nt.getEntry("mode").setString
self.__nt.putBoolean("is_simulation", self.isSimulation())
self.__nt_put_is_ds_attached(self.ds.isDSAttached())
self.__done = False
# cache these
self.__sd_update = wpilib.SmartDashboard.updateValues
self.__lv_update = wpilib.LiveWindow.getInstance().updateValues
# self.__sf_update = Shuffleboard.update
self.watchdog = SimpleWatchdog(self.control_loop_wait_time)
def robotInit(self):
"""
Define all motor controllers, joysticks, Pneumatics, etc. here so you can use them in teleop/auton
"""
self.drive_motor1 = wpilib.Talon(
0) # <--- or whatever motor controller you are using
self.drive_motor2 = wpilib.Talon(1)
self.robot_drive = wpilib.RobotDrive(
self.drive_motor1, self.drive_motor2
) # <--- says to robot that these motors work together to drive robot
self.xboxController = wpilib.Joystick(
0) # <--- joystick, does not have to be an xbox controller
self.components = { # Add all the objects you are going to want in autonomous like sensors, the robot drive, etc.
'drive': self.
robot_drive #give it a nickname as well. In this case, we "nicknamed" self.robot_drive as 'drive' so in auto you will do self.drive
}
self.automodes = AutonomousModeSelector(
'auto-modes', self.components
) #pass in the folder with all your auto modes and the components you want in auto
def robotInit(self):
"""
Motors
"""
if not wpilib.RobotBase.isSimulation():
import ctre
self.motor1 = ctre.CANTalon(1) #Drive Motors
self.motor2 = ctre.CANTalon(2)
self.motor3 = ctre.CANTalon(3)
self.motor4 = ctre.CANTalon(4)
else:
self.motor1 = wpilib.Talon(1) #Drive Motors
self.motor2 = wpilib.Talon(2)
self.motor3 = wpilib.Talon(3)
self.motor4 = wpilib.Talon(4)
self.climb1 = wpilib.VictorSP(7)
self.climb2 = wpilib.VictorSP(8)
"""
Spike Relay for LED
"""
self.ledRing = wpilib.Relay(
0, wpilib.Relay.Direction.kForward) #Only goes forward voltage
"""
Sensors
"""
self.navx = navx.AHRS.create_spi() #the Gyro
self.psiSensor = wpilib.AnalogInput(2)
self.powerBoard = wpilib.PowerDistributionPanel(0) #Might need or not
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.encoder = wpilib.Encoder(2, 3)
self.switch = wpilib.DigitalInput(6)
self.servo = wpilib.Servo(0)
self.joystick = wpilib.Joystick(0) #xbox controller
wpilib.CameraServer.launch('misc/vision.py:main')
"""
Buttons
"""
self.visionEnable = wpilib.buttons.JoystickButton(self.joystick,
7) #X button
self.gearPistonButton = wpilib.buttons.JoystickButton(self.joystick, 5)
self.safetyPistonButton = wpilib.buttons.JoystickButton(
self.joystick, 3)
self.controlSwitch = button_debouncer.ButtonDebouncer(
self.joystick, 8,
period=0.5) #Controll switch init for auto lock direction
self.driveControlButton = button_debouncer.ButtonDebouncer(
self.joystick, 1, period=0.5) #Mecanum to tank and the other way
self.climbReverseButton = wpilib.buttons.JoystickButton(
self.joystick, 2) #Button for reverse out of climb
"""
Solenoids
"""
self.drivePiston = wpilib.DoubleSolenoid(
3, 4) #Changes us from mecanum to hi-grip
self.gearPiston = wpilib.Solenoid(2)
self.safetyPiston = wpilib.Solenoid(1)
self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4,
self.motor3, self.motor2)
self.Drive = drive.Drive(self.robodrive, self.drivePiston, self.navx,
self.encoder, self.ledRing)
self.climber = climb.Climb(self.climb1, self.climb2)
"""
All the variables that need to be defined
"""
self.motorWhere = True #IF IT IS IN MECANUM BY DEFAULT
self.rotationXbox = 0
self.climbVoltage = 0
"""
Timers
"""
self.timer = wpilib.Timer()
self.timer.start()
self.vibrateTimer = wpilib.Timer()
self.vibrateTimer.start()
self.vibrator = vibrator.Vibrator(self.joystick, self.vibrateTimer,
.25, .15)
"""
The great NetworkTables part
"""
self.vision_table = NetworkTable.getTable('/GRIP/myContoursReport')
self.alignGear = alignGear.AlignGear(self.vision_table)
self.robotStats = NetworkTable.getTable('SmartDashboard')
self.matchTime = matchTime.MatchTime(self.timer, self.robotStats)
"""
Drive Straight
"""
self.DS = driveStraight.driveStraight(self.timer, self.vibrator,
self.Drive, self.robotStats)
self.components = {
'drive': self.Drive,
'alignGear': self.alignGear,
'gearPiston': self.gearPiston,
'ultrasonic': self.ultrasonic
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.updater()
def robotInit(self):
#Networktables
self.netTable = NetworkTables.getTable('SmartDashboard')
#Hud Data Handlers
self.statUpdater = SU.StatusUpdater(self, self.netTable)
#Camera Server
wpilib.CameraServer.launch()
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(9)
self.motor4 = ctre.WPI_TalonSRX(10)
#Intake Motors
self.stage1Left = ctre.WPI_TalonSRX(5)
self.stage1Right = ctre.WPI_TalonSRX(6)
self.stage2Left = ctre.WPI_TalonSRX(4)
self.stage2Right = ctre.WPI_TalonSRX(7)
self.stage3Left = ctre.WPI_TalonSRX(3)
self.stage3Right = ctre.WPI_TalonSRX(8)
#Pan Arm Controls
self.leftPanArm = wpilib.PWMVictorSPX(0)
self.rightPanArm = wpilib.PWMVictorSPX(1)
#Shifters
self.shifter = wpilib.DoubleSolenoid(1, 2)
#Climb
self.pto = wpilib.DoubleSolenoid(3, 4)
self.climbLift = wpilib.Solenoid(5)
#User Inputs
self.playerOne = wpilib.XboxController(0)
self.playerTwo = wpilib.XboxController(1)
#Navx
self.navx = navx.AHRS.create_spi()
self.power_board = wpilib.PowerDistributionPanel()
#User Inputs
self.playerOne = wpilib.XboxController(0)
self.playerTwo = wpilib.XboxController(1)
#Navx
self.navx = navx.AHRS.create_spi()
#Points
#self.points = []
#Setup Logic
self.rightDriveMotors = wpilib.SpeedControllerGroup(
self.motor3, self.motor4)
self.leftDriveMotors = wpilib.SpeedControllerGroup(
self.motor1, self.motor2)
self.leftDriveMotors.setInverted(True)
self.robotDrive = DifferentialDrive(self.leftDriveMotors,
self.rightDriveMotors)
self.lowerIntakeMotors = wpilib.SpeedControllerGroup(
self.stage1Left, self.stage1Right, self.stage2Left,
self.stage2Right)
self.stage3 = wpilib.SpeedControllerGroup(self.stage3Left,
self.stage3Right)
if wpilib.SolenoidBase.getPCMSolenoidVoltageStickyFault(0) == True:
wpilib.SolenoidBase.clearAllPCMStickyFaults(0)
self.pto.set(wpilib.DoubleSolenoid.Value.kReverse)
#Drive.py init
self.drive = drive.Drive(self.robotDrive, self.navx, self.motor1,
self.motor2, self.motor3, self.motor4,
self.shifter)
#Intake.py
self.intake = intake.Intake(self.lowerIntakeMotors, self.stage3,
self.leftPanArm, self.rightPanArm)
#Driver Station Instance
self.driverStation = wpilib.DriverStation.getInstance()
#Auto mode variables
self.components = {'drive': self.drive, 'intake': self.intake}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# hello
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.sweeper_relay = wpilib.Relay(0)
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.can_motor = wpilib.CANTalon(15)
self.sensor = Sensor(self.tote_motor, self.can_motor)
self.tote_forklift = ToteForklift(self.tote_motor, self.sensor, 2)
self.can_forklift = CanForklift(self.can_motor, self.sensor, 3)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.next_pos = 1
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift,
self.drive)
self.components = {
'tote_forklift': self.tote_forklift,
'can_forklift': self.can_forklift,
'drive': self.drive,
'align': self.align,
'sensors': self.sensor
}
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.components)
# #Defining Buttons##
self.canUp = Button(self.joystick1, 3)
self.canDown = Button(self.joystick1, 2)
self.canTop = Button(self.joystick1, 6)
self.canBottom = Button(self.joystick1, 7)
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.ui_joystick_can_up = Button(self.ui_joystick, 5)
self.ui_joystick_can_down = Button(self.ui_joystick, 3)
self.reverseDirection = Button(self.joystick1, 1)
#self.alignTrigger = Button(self.joystick2, 1)
self.toteTo = 0
self.oldTote = 0
self.canTo = 0
self.oldCan = 0
self.reverseRobot = False
self.oldReverseRobot = False
self.autoLift = False
self.sd.putNumber('liftTo', 0)
self.sd.putNumber('binTo', 0)
self.sd.putBoolean('autoLift', False)
self.sd.putBoolean('reverseRobot', False)
def robotInit(self):
'''Robot-wide Initialization code'''
self.robotnumber = wpilib.DigitalInput(9)
#Initializing networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('robot_speed', 1)
self.smart_dashboard.putString('robot_name',
"Zil1" if self.robotnumber else "Zil2")
# initialize and launch the camera
wpilib.CameraServer.launch()
# initialize the left and right encoders.
self.encoder_wheel_left = wpilib.Encoder(
0, 1, True, wpilib.Encoder.EncodingType.k4X)
self.encoder_wheel_left.reset()
self.encoder_wheel_right = wpilib.Encoder(
2, 3, False, wpilib.Encoder.EncodingType.k4X)
self.encoder_wheel_right.reset()
self.encoder_wheel_left.setDistancePerPulse(self.kDistancePerPulse)
self.encoder_wheel_right.setDistancePerPulse(self.kDistancePerPulse)
#Initalizing drive motors
if self.robotnumber:
self.drive_l_motor = wpilib.Victor(portmap.motors.left_drive)
self.drive_r_motor = wpilib.Victor(portmap.motors.right_drive)
else:
self.drive_l_motor = wpilib.Spark(portmap.motors.left_drive)
self.drive_r_motor = wpilib.Spark(portmap.motors.right_drive)
self.claw_l_motor = wpilib.Victor(portmap.motors.left_claw)
self.claw_r_motor = wpilib.Victor(portmap.motors.right_claw)
self.lift_motor = wpilib.Victor(portmap.motors.lift)
# initialize drive (differential drive is tank drive)
self.drive = wpilib.drive.DifferentialDrive(self.drive_l_motor,
self.drive_r_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(wpilib.AnalogInput(1))
self.gyro.calibrate()
# initialize the ultra sonic
self.range = wpilib.AnalogInput(0)
# initialize Accelerometer
self.accelerometer = wpilib.BuiltInAccelerometer(
Accelerometer.Range.k4G)
# initialize autonomous components
self.components = {
'drive': self.drive,
'drive_r_motor': self.drive_r_motor,
'drive_l_motor': self.drive_l_motor,
'claw_r_motor': self.claw_r_motor,
'claw_l_motor': self.claw_r_motor,
'lift_motor': self.lift_motor,
'encoder_wheel_right': self.encoder_wheel_right,
'encoder_wheel_left': self.encoder_wheel_left,
'gyro': self.gyro,
'accelerometer': self.accelerometer,
'range': self.range
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.logger.log(logging.INFO, "robot initialization complete.")
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# joystick 1 on the driver station
self.stick = wpilib.XboxController(0)
self.driverStation = wpilib.DriverStation
self.frontRight = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.rearRight = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.frontLeft = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.rearLeft = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight.setExpiration(0.2)
self.rearRight.setExpiration(0.2)
self.frontRight.setExpiration(0.2)
self.rearLeft.setExpiration(0.2)
self.drive = DifferentialDrive(self.left, self.right)
# initialize motors other than drive
self.intakeRight = wpilib.VictorSP(0)
self.elevator = ctre.wpi_talonsrx.WPI_TalonSRX(
5) # Talon SRX controller with CAN address 5
self.intakeLeft = wpilib.VictorSP(2)
self.battleAxe = wpilib.VictorSP(3)
self.actuator = wpilib.Spark(4)
self.axeExtender = wpilib.Spark(5)
######################################
self.encoderTicksPerInch = 1159
self.elevator.setQuadraturePosition(0, 0)
self.elevator.configForwardSoftLimitThreshold(
int(round(-0.1 * self.encoderTicksPerInch)), 10)
self.elevator.configReverseSoftLimitThreshold(
int(round(-39.5 * self.encoderTicksPerInch)), 10)
self.elevator.configForwardSoftLimitEnable(True, 10)
self.elevator.configReverseSoftLimitEnable(True, 10)
self.elevator.configPeakOutputForward(0.8, 10)
self.elevator.configPeakOutputReverse(-1, 10)
self.elevator.set(ctre.ControlMode.Position, 0)
self.elevator.selectProfileSlot(0, 0)
self.elevator.config_kF(0, 0, 10)
self.elevator.config_kP(0, 0.6, 10)
self.elevator.config_kI(0, 0.003, 10)
self.elevator.config_kD(0, 0, 10)
self.elevator.config_IntegralZone(0, 100, 10)
self.elevator.configAllowableClosedloopError(
0, int(round(0.01 * self.encoderTicksPerInch)), 10)
# initialize limit switches and hall-effect sensors
self.actuatorSwitchMin = wpilib.DigitalInput(0)
self.actuatorSwitchMax = wpilib.DigitalInput(1)
self.battleAxeSwitchUp = wpilib.DigitalInput(2)
self.battleAxeSwitchDown = wpilib.DigitalInput(3)
self.battleAxeExtenderSwitch = wpilib.DigitalInput(4)
self.elevatorZeroSensor = wpilib.DigitalInput(5)
self.powerDistributionPanel = wpilib.PowerDistributionPanel()
self.powerDistributionPanel.resetTotalEnergy()
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.navxSensor = navx.AHRS.create_spi()
# self.navxSensor = navx.AHRS.create_i2c()
# Items in this dictionary are available in your autonomous mode
# as attributes on your autonomous object
self.components = {
'drive': self.drive,
'navxSensor': self.navxSensor,
'actuator': self.actuator,
'actuatorSwitchMin': self.actuatorSwitchMin,
'actuatorSwitchMax': self.actuatorSwitchMax,
'elevator': self.elevator,
'intakeRight': self.intakeRight,
'intakeLeft': self.intakeLeft,
'gameData': self.driverStation.getInstance()
}
# * The first argument is the name of the package that your autonomous
# modes are located in
# * The second argument is passed to each StatefulAutonomous when they
# start up
self.automodes = AutonomousModeSelector('autonomous', self.components)
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.CameraServer.launch('vision.py:main')
