def __init__(self): super().__init__() self.frontLeft = wpilib.Spark(0) self.rearLeft = wpilib.Spark(3) self.frontRight = wpilib.Spark(1) self.rearRight = wpilib.Spark(2) self.drive = wpilib.drive.MecanumDrive(self.frontLeft, self.rearLeft, self.frontRight, self.rearRight) self.left_ultra = wpilib.Ultrasonic(5, 4) self.right_ultra = wpilib.Ultrasonic(3, 2) self.front_ultra = wpilib.Ultrasonic(1, 0) self.left_ultra.setAutomaticMode(True) self.right_ultra.setAutomaticMode(True) self.front_ultra.setAutomaticMode(True)
def createObjects(self): NetworkTables.initialize() wpilib.CameraServer.launch() self.auto_left_motor = self.left_front_motor = ctre.WPI_TalonSRX(1) self.left_rear_motor = ctre.WPI_VictorSPX(2) self.left_rear_motor.follow(self.auto_left_motor) self.auto_right_motor = self.right_front_motor = ctre.WPI_TalonSRX(3) self.right_rear_motor = ctre.WPI_VictorSPX(4) self.right_rear_motor.follow(self.auto_right_motor) self.auto_right_motor.setSensorPhase(True) self.grippers_left_motor = ctre.WPI_VictorSPX(8) self.grippers_right_motor = ctre.WPI_VictorSPX(7) self.left = wpilib.SpeedControllerGroup(self.left_front_motor, self.left_rear_motor) self.right = wpilib.SpeedControllerGroup(self.right_front_motor, self.right_rear_motor) self.drive = wpilib.drive.DifferentialDrive(self.left, self.right) self.hatch_panel_piston_solenoid = wpilib.DoubleSolenoid(1, 0) self.gripper_piston_solenoid = wpilib.DoubleSolenoid(2, 3) self.panel_mechanism_piston_solenoid = wpilib.DoubleSolenoid(4, 5) self.first_hatch_panel_piston_solenoid = wpilib.Solenoid(7) self.ramp_pistons_solenoid = wpilib.Solenoid(6) self.ramp_motor = ctre.WPI_TalonSRX(6) self.ramp_cfg = MotorConfig(motor=self.ramp_motor, speed=1.0) self.range_sensor = wpilib.Ultrasonic( 0, 1, wpilib.Ultrasonic.Unit.kMillimeters) self.range_sensor.setAutomaticMode(True) self.right_joystick = wpilib.Joystick(0) self.left_joystick = wpilib.Joystick(1) self.controller = wpilib.XboxController(2) self.auto_aligner_button = wpilib.buttons.JoystickButton( self.right_joystick, 2) self.position_align_button = wpilib.buttons.JoystickButton( self.left_joystick, 2) self.use_teleop_in_autonomous = True
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): self.drive1 = wpilib.Talon(0) self.drive2 = wpilib.Talon(1) self.shooter = wpilib.Talon(2) self.cam = wpilib.Talon(3) #Solenoid me self.arm1 = wpilib.DoubleSolenoid(0, 1, 2) self.arm2 = wpilib.DoubleSolenoid(0, 3, 4) #Testing some ultrasonic sensors self.ultrasonic = wpilib.Ultrasonic(1, 0, units.inch) self.ultrasonic.setAutomaticMode(True) #TWO CONTROLLERS self.controller = wpilib.Joystick(0) self.second_controller = wpilib.Joystick(1) #A button self.joystick_button = wpilib.buttons.JoystickButton( self.second_controller, 1) #B Button self.second_button = wpilib.buttons.JoystickButton( self.second_controller, 2) #Right bumper self.right_bumper = wpilib.buttons.JoystickButton( self.second_controller, 6) #Right bumper for boost on main controller self.main_fast = wpilib.buttons.JoystickButton(self.controller, 6) #Saving for later #Utrasonic Sensor #self.sensor = wpilib.AnalogInput(3) #self.ultrasonic = xl_max_sonar_ez.MaxSonarEZAnalog(3, units.inch) #Make all the variables needed self.shooter_piston = 1 self.speedShooter = 0 self.speedCam = 0 #Init variable for ultrasonic sensor #Shooter speeds self.shooter_high = .5 self.updater()
def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1) self.btn_intake_ = JoystickButton(self.joystick_alt, 5) self.btn_align = JoystickButton(self.joystick_left, 1) self.btn_intake_in = JoystickButton(self.joystick_alt, 3) self.btn_intake_out = JoystickButton(self.joystick_alt, 4) self.btn_cp_extend = Toggle(self.joystick_left, 4) self.btn_winch = JoystickButton(self.joystick_alt, 8) self.btn_cp_motor = JoystickButton(self.joystick_left, 3) self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12) self.btn_launcher_idle = Toggle(self.joystick_alt, 10) self.btn_launcher_motor_close = JoystickButton(self.joystick_alt, 11) # Initiation Line self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9) self.btn_slow_movement = JoystickButton(self.joystick_right, 1) self.btn_intake_solenoid = Toggle(self.joystick_alt, 2) self.btn_scissor_extend = Toggle(self.joystick_alt, 7) self.btn_color_sensor = JoystickButton(self.joystick_left, 5) self.btn_cp_stop = JoystickButton(self.joystick_left, 2) self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6) self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1) self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6) # Set up motors for encoders self.master_left = CANSparkMax(1, MotorType.kBrushless) self.master_right = CANSparkMax(4, MotorType.kBrushless) self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi self.left_encoder = self.master_left.getEncoder() self.left_encoder.setPositionConversionFactor(self.encoder_constant) self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.right_encoder = self.master_right.getEncoder() self.right_encoder.setPositionConversionFactor(self.encoder_constant) self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( self.master_left, CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup( self.master_right, CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Winch self.winch_motors = wpilib.SpeedControllerGroup( CANSparkMax(8, MotorType.kBrushless), CANSparkMax(9, MotorType.kBrushless)) self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7) self.hook_motor = WPI_VictorSPX(3) # Control Panel Spinner self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.cp_solenoid = wpilib.DoubleSolenoid( 5, 4) # Reversed numbers so kForward is extended self.cp_motor = WPI_VictorSPX(2) self.ultrasonic = Ultrasonic(3, 4) self.ultrasonic.setAutomaticMode(True) self.ultrasonic.setEnabled(True) # Intake self.intake_motor = WPI_VictorSPX(1) self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed) self.intake_solenoid = wpilib.DoubleSolenoid(2, 1) self.intake_switch = wpilib.DigitalInput(2) # Launcher # self.launcher_spark = CANSparkMax(40, MotorType.kBrushed) # self.launcher_spark.setInverted(True) self.launcher_motor = CANSparkMax(10, MotorType.kBrushed) self.launcher_motor.setInverted(True) self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed) self.launcher_motor_follower.follow(self.launcher_motor) self.launcher_solenoid = wpilib.Solenoid(0) # Don't use index pin and change to k1X encoding to decrease rate measurement jitter self.launcher_encoder = self.launcher_motor.getEncoder( CANEncoder.EncoderType.kQuadrature, 8192) self.rpm_controller = self.launcher_motor.getPIDController() self.launcher_sensor = wpilib.Ultrasonic(0, 1) self.launcher_sensor.setAutomaticMode(True) self.launcher_sensor.setEnabled(True) self.launcher_encoder.setPosition(0) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Limelight self.limelight = Limelight() # Camera Stream CameraServer.launch('camera/camera.py:main') # Robot motion control self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code self.drive_feedforward = DRIVE_FEEDFORWARD self.trajectories = load_trajectories() # LED strip self.led_driver = BlinkinLED(0)
def robotInit(self): # NetworkTables.initialize() # self.sd = NetworkTables.getTable('SmartDashboard') wpilib.CameraServer.launch() # Inicializadores_de_PCM (en caso de que no arranque el PCM) # self.Compressor.setClosedLoopControl(True) # self.enabled = self.Compressor.enabled() #Solenoides y Compresores self.Compressor = wpilib.Compressor(0) self.PSV = self.Compressor.getPressureSwitchValue() self.piston = wpilib.Solenoid(0, 0) self.impulsor_frontal = wpilib.DoubleSolenoid(0, 2, 3) self.impulsor_trasero = wpilib.DoubleSolenoid(0, 4, 5) # Encoders y otros Sensores self.encoder = wpilib.Encoder(8, 7) self.left_sensor = wpilib.DigitalInput(0) self.principal_sensor = wpilib.DigitalInput(1) self.right_sensor = wpilib.DigitalInput(2) self.ultrasonic = wpilib.Ultrasonic(3, 4) self.prueba_sensor = wpilib.DigitalInput(5) self.P = 0.2 self.I = 0 self.D = 0 self.integral = 0 self.previous_error = 0 # Contador y Control self.timer = wpilib.Timer() # Motores del Chasis self.front_left_motor = wpilib.Talon(0) self.rear_left_motor = wpilib.Talon(1) self.front_right_motor = wpilib.Talon(2) self.rear_right_motor = wpilib.Talon(3) #lift and claw motors self.lift_motor = wpilib.Talon(4) self.lift_motor_2 = wpilib.Talon(5) #Union de los motores para su funcionamiento # en conjunto de mecaunm self.drive = MecanumDrive(self.front_left_motor, self.rear_left_motor, self.front_right_motor, self.rear_right_motor) #Motor impulsor self.motor_impulsor = wpilib.Talon(6)
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()