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()