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