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):
#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 __init__ (self):
'''
Constructor.
'''
super().__init__()
print("Team 1418 robot code for 2014")
#################################################################
# THIS CODE IS SHARED BETWEEN THE MAIN ROBOT AND THE ELECTRICAL #
# TEST CODE. WHEN CHANGING IT, CHANGE BOTH PLACES! #
#################################################################
wpilib.SmartDashboard.init()
# Joysticks
self.joystick1 = wpilib.Joystick(1)
self.joystick2 = wpilib.Joystick(2)
# Motors
self.lf_motor = wpilib.Jaguar(1)
self.lf_motor.label = 'lf_motor'
self.lr_motor = wpilib.Jaguar(2)
self.lr_motor.label = 'lr_motor'
self.rr_motor = wpilib.Jaguar(3)
self.rr_motor.label = 'rr_motor'
self.rf_motor = wpilib.Jaguar(4)
self.rf_motor.label = 'rf_motor'
self.winch_motor = wpilib.CANJaguar(5)
self.winch_motor.label = 'winch'
self.intake_motor = wpilib.Jaguar(6)
self.intake_motor.label = 'intake'
# Catapult gearbox control
self.gearbox_solenoid=wpilib.DoubleSolenoid(2, 1)
self.gearbox_solenoid.label = 'gearbox'
# Arm up/down control
self.vent_bottom_solenoid = wpilib.Solenoid(3)
self.vent_bottom_solenoid.label = 'vent bottom'
self.fill_bottom_solenoid = wpilib.Solenoid(4)
self.fill_bottom_solenoid.label = 'fill bottom'
self.fill_top_solenoid = wpilib.Solenoid(5)
self.fill_top_solenoid.label = 'fill top'
self.vent_top_solenoid = wpilib.Solenoid(6)
self.vent_top_solenoid.label = 'vent top'
self.pass_solenoid = wpilib.Solenoid(7)
self.pass_solenoid.label = 'pass'
self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor, self.lf_motor, self.rf_motor)
self.robot_drive.SetSafetyEnabled(False)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kFrontLeftMotor, True)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kRearLeftMotor, True)
# Sensors
self.gyro = wpilib.Gyro(1)
self.ultrasonic_sensor = wpilib.AnalogChannel(3)
self.ultrasonic_sensor.label = 'Ultrasonic'
self.arm_angle_sensor = wpilib.AnalogChannel(4)
self.arm_angle_sensor.label = 'Arm angle'
self.ball_sensor = wpilib.AnalogChannel(6)
self.ball_sensor.label = 'Ball sensor'
self.accelerometer = wpilib.ADXL345_I2C(1, wpilib.ADXL345_I2C.kRange_2G)
self.compressor = wpilib.Compressor(1,1)
#################################################################
# END SHARED CODE #
#################################################################
#
# Initialize robot components here
#
self.drive = drive.Drive(self.robot_drive, self.ultrasonic_sensor,self.gyro)
self.initSmartDashboard()
self.pushTimer=wpilib.Timer()
self.catapultTimer=wpilib.Timer()
self.catapult=catapult.Catapult(self.winch_motor,self.gearbox_solenoid,self.pass_solenoid,self.arm_angle_sensor,self.ball_sensor,self.catapultTimer)
self.intakeTimer=wpilib.Timer()
self.intake=intake.Intake(self.vent_top_solenoid,self.fill_top_solenoid,self.fill_bottom_solenoid,self.vent_bottom_solenoid,self.intake_motor,self.intakeTimer)
self.pulldowntoggle=False
self.components = {
'drive': self.drive,
'catapult': self.catapult,
'intake': self.intake
}
self.control_loop_wait_time = 0.025
self.autonomous = AutonomousModeManager(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):
#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):
"""
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()