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.elevator_motor = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.elevator_follower = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.elevator_follower.follow(self.elevator_motor)
self.chomp_relay = wpilib.Relay(3)
self.servo = wpilib.Servo(2)
self.timer = wpilib.Timer()
self.initial_time = 0
self.chomp_pressed = False
self.chomp_position = 0
self.last_button_value = False
def __init__(self):
super().__init__()
self.dog = self.GetWatchdog()
self.stick1 = wpilib.Joystick(1)
self.stick2 = wpilib.Joystick(2)
self.stick3 = wpilib.Joystick(3)
self.leftArmStick = wpilib.KinectStick(1)
self.rightArmStick = wpilib.KinectStick(2)
self.motor1 = wpilib.CANJaguar(1)
self.motor2 = wpilib.CANJaguar(2)
self.leftArm = wpilib.Servo(1)
self.rightArm = wpilib.Servo(2)
self.leftLeg = wpilib.Servo(3)
self.rightLeg = wpilib.Servo(4)
self.spinner = wpilib.Servo(5)
self.compressor = wpilib.Compressor(1, 1)
self.compressor.Start()
self.relayMotor = wpilib.Relay(2)
self.solenoid1 = wpilib.Solenoid(1)
self.solenoid2 = wpilib.Solenoid(2)
def __init__(self):
super().__init__()
self.winchL = ctre.CANTalon(robotMap.WINCHL)
print("winch l created")
self.winchR = ctre.CANTalon(robotMap.WINCHR)
print("winch r created")
self.led = wpilib.Relay(robotMap.LED)
def __init__(self):
print('HatchGrab: init called')
super().__init__('HatchGrab')
self.logPrefix = "HatchGrab: "
try:
self.hatchGrabSolenoid = wpilib.DoubleSolenoid(1, robotmap.hatchgrab.solenoidExtendPort, robotmap.hatchgrab.solenoidRetractPort)
except Exception as e:
print("{}Exception caught instantiating hatch grabber solenoid. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
self.hatchGrabExtendSol = wpilib.Relay(robotmap.hatchgrab.extensionSpikePort, 0)
self.hatchToggle = False
self.hatchExtendToggle = False
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 __init__(self, port):
self.rel = wpi.Relay(port)
self.port = port
comp_needed = True
print("ghetto relay class set up on port " + str(port))
def __init__(self):
super().__init__()
self.winchL = ctre.CANTalon(robotMap.WINCHL)
self.winchR = ctre.CANTalon(robotMap.WINCHR)
self.led = wpilib.Relay(robotMap.LED)
self.pdp = PowerDistributionPanel(0)
def createObjects(self):
# Joysticks
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# Motors (l/r = left/right, f/r = front/rear)
self.lf_motor = wpilib.CANTalon(5)
self.lr_motor = wpilib.CANTalon(10)
self.rf_motor = wpilib.CANTalon(15)
self.rr_motor = wpilib.CANTalon(20)
# Drivetrain object
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
# Left and right arm motors (there's two, which both control the raising and lowering the arm)
self.leftArm = wpilib.CANTalon(25)
self.rightArm = wpilib.CANTalon(30)
# Motor that spins the bar at the end of the arm.
# There was originally going to be one on the right, but we decided against that in the end.
# In retrospect, that was probably a mistake.
self.leftBall = wpilib.Talon(9)
# Motor that reels in the winch to lift the robot.
self.winchMotor = wpilib.Talon(0)
# Motor that opens the winch.
self.kickMotor = wpilib.Talon(1)
# Aiming flashlight
self.flashlight = wpilib.Relay(0)
# Timer to keep light from staying on for too long
self.lightTimer = wpilib.Timer()
# Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off.
# self.turningOffState keeps track of which on/off it's on.
self.turningOffState = 0
# Is currently on or off? Used to detect if UI button is pressed.
self.lastState = False
# Drive encoders; measure how much the motor has spun
self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True)
self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor)
# Distance sensors
self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0)
self.ultrasonic = wpilib.AnalogInput(1)
# NavX (purple board on top of the RoboRIO)
self.navX = navx.AHRS.create_spi()
# Initialize SmartDashboard, the table of robot values
self.sd = NetworkTable.getTable('SmartDashboard')
# How much will the control loop pause in between (0.025s = 25ms)
self.control_loop_wait_time = 0.025
# Button to reverse controls
self.reverseButton = ButtonDebouncer(self.joystick1, 1)
# Initiate functional buttons on joysticks
self.shoot = ButtonDebouncer(self.joystick2, 1)
self.raiseButton = ButtonDebouncer(self.joystick2, 3)
self.lowerButton = ButtonDebouncer(self.joystick2, 2)
self.lightButton = ButtonDebouncer(self.joystick1, 6)
class variableInitalize():
#Joysticks
stick = wpilib.Joystick(1)
#Visctors
DriveL1 = wpilib.Victor(2)
DriveL2 = wpilib.Victor(4)
DriveR1 = wpilib.Victor(1)
DriveR2 = wpilib.Victor(3)
shooterRoller = wpilib.Victor(9)
shooterNeck = wpilib.Victor(6)
bridgeWedge = wpilib.Victor(10)
elevator = wpilib.Victor(5)
frontFeed = wpilib.Victor(7)
backFeed = wpilib.Victor(8)
#Encoders
neckEncode = wpilib.Encoder(5, 6, True)
leftDriveEncode = wpilib.Encoder(3, 4, False)
rightDriveEncode = wpilib.Encoder(1, 2, False)
turretroller = wpilib.Encoder(7, 8, False)
#Relays
camLights = wpilib.Relay(1)
targetInd = wpilib.Relay(4)
compLights = wpilib.Relay(2)
#Timers
neckTimer = wpilib.Timer()
ballTimer = wpilib.Timer()
feedTimer = wpilib.Timer()
neckcTimer = wpilib.Timer()
rpmTime = wpilib.Timer()
rpmlockedTimer = wpilib.Timer()
autonTimer = wpilib.Timer()
rpmLockFlash = wpilib.Timer()
rpmTimer = wpilib.Timer()
#Digital Input
frontFeedDI = wpilib.DigitalInput(9)
backFeedDI = wpilib.DigitalInput(10)
bottomNeckDI = wpilib.DigitalInput(11)
topNeckDI = wpilib.DigitalInput(12)
#Digital Output
#trackingLight = wpilib.DigitalOutput(12)
ledNotification = wpilib.DigitalOutput(13)
#Analong
rpmPot = wpilib.AnalogChannel(1)
bridgePot = wpilib.AnalogChannel(2)
#Solinoid
breakRelay = wpilib.Solenoid(1)
#drive station
ds = wpilib.DriverStationLCD.GetInstance()
rateOfPot = potRate(rpmPot, rpmTimer)
def Wait(self, waitTime):
wpilib.Wait(waitTime)
def SimpleRobot(self):
return wpilib.SimpleRobot
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):
"""
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 __init__(self, robot):
super().__init__()
self.robot = robot
self.spike = wpilib.Relay(1)
def __init__(self):
super().__init__()
self.winchL = ctre.CANTalon(robotMap.WINCHL)
self.winchR = ctre.CANTalon(robotMap.WINCHR)
self.led = wpilib.Relay(robotMap.LED)