def __init__(self):
self.topSpinner = ctre.WPI_TalonSRX(21)
self.topSpinner.configFactoryDefault(0)
self.bottomSpinner = ctre.WPI_TalonSRX(20)
self.bottomSpinner.configFactoryDefault(0)
# TODO fix names
self.solenoid0 = wpilib.Solenoid(0)
self.solenoid1 = wpilib.Solenoid(1)
self.solenoid2 = wpilib.Solenoid(2)
self.solenoid3 = wpilib.Solenoid(3)
self.compressor = wpilib.Compressor(0)
self.stopCompressor()
self.armOut = False
self.grabOut = False
self.slideMotor = ctre.WPI_TalonSRX(30)
self.slideMotor.configFactoryDefault(0)
self.slideMotor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0)
self.slideMotor.setSensorPhase(False)
self.slideMotor.config_kP(0, 0.3, 0)
self.slideMotor.config_kI(0, 0, 0)
self.slideMotor.config_kD(0, 3, 0)
self.slideMotor.config_kF(0, 0, 0)
self.slideMotor.configPeakOutputReverse(-0.75, 0)
self.slideValue = None
self.resetAllSensors()
def __init__(self, roller_motor, pivot, stick, initial_state):
self.roller_motor = roller_motor
self.pivot = pivot
self.stick = stick
self.rollerenabler = JoystickButton(self.stick, 14)
self.pivotupbutton = JoystickButton(self.stick, 5)
self.pivotdownbutton = JoystickButton(self.stick, 10)
self.pistonoutbutton = JoystickButton(self.stick, 8)
self.pistoninbutton = JoystickButton(self.stick, 7)
#self.ratchetswitchbutton = JoystickButton(self.stick, 15)
#self.ratchetbtnpressed = False
#self.pivotstopbutton = JoystickButton(self.stick, 14)
#self.pivotinitbutton = JoystickButton(self.stick, 13)
self.rollerout = JoystickButton(self.stick, 9)
self.rollerin = JoystickButton(self.stick, 6)
self.state = initial_state
self.pivot.selectProfileSlot(0, 0)
self.switch = wpilib.DigitalInput(2)
self.count = 0
self.sol2 = wpilib.Solenoid(5, 3)
self.sol1 = wpilib.Solenoid(5, 0)
self.sol1.set(False)
self.sol2.set(True)
self.ratchet = wpilib.Solenoid(5, 6)
self.ratchetEnabled = False
self.ratchet.set(not self.ratchetEnabled)
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(48),
drive_talon=ctre.TalonSRX(41),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(51),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.TalonSRX(52),
drive_talon=ctre.TalonSRX(53),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.TalonSRX(14)
self.intake_right_motor = ctre.TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(2)
self.intake_kicker = wpilib.Solenoid(3)
self.left_extension = wpilib.Solenoid(6)
self.right_extension = wpilib.DoubleSolenoid(forwardChannel=5,
reverseChannel=4)
self.compressor = wpilib.Compressor()
self.lifter_motor = ctre.TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
self.intake_cube_switch = wpilib.DigitalInput(3)
# create the imu object
self.imu = IMU('navx')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
self.sd = NetworkTables.getTable("SmartDashboard")
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.frontLeft = ctre.WPI_TalonSRX(1)
self.frontRight = ctre.WPI_TalonSRX(3)
self.rearLeft = ctre.WPI_TalonSRX(2)
self.rearRight = ctre.WPI_TalonSRX(4)
self.spool = ctre.WPI_TalonSRX(5)
# self.rearLeft.setInverted(True)
# self.frontLeft.setInverted(True)
self.drive = wpilib.RobotDrive(self.frontLeft, self.rearLeft,
self.frontRight, self.rearRight)
self.stick = wpilib.Joystick(0)
self.pickUpHandler = 0
self.pickupTimer = 0
self.picker = wpilib.Solenoid(0, 1)
self.rotation = wpilib.Solenoid(0, 2)
self.thrust = wpilib.Solenoid(0, 3)
self.transmission = wpilib.Solenoid(0, 4)
def init(self):
self.logger.info("DeepSpaceDrive::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.current_state = DriveState.OPERATOR_CONTROL
self.pigeon = PigeonIMU(robotmap.PIGEON_IMU_CAN_ID)
self.leftTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_MASTER_CAN_ID)
self.leftTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_SLAVE_CAN_ID)
self.rightTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_MASTER_CAN_ID)
self.rightTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_SLAVE_CAN_ID)
self.dummyTalon = ctre.TalonSRX(robotmap.CLAW_LEFT_WHEELS_CAN_ID)
self.talons = [self.leftTalonMaster, self.leftTalonSlave, self.rightTalonMaster, self.rightTalonSlave]
self.masterTalons = [self.leftTalonMaster, self.rightTalonMaster]
self.drive = wpilib.RobotDrive(self.leftTalonMaster, self.rightTalonMaster)
self.drive.setSafetyEnabled(False)
self.drive_front_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_EXTEND_SOLENOID)
self.drive_front_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_RETRACT_SOLENOID)
self.drive_back_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_EXTEND_SOLENOID)
self.drive_back_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_RETRACT_SOLENOID)
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 robotInit(self):
"""Robot initialization function"""
# variables for managing pneumatics sequence
self.State = -1
self.wait_timer = wpilib.Timer()
# object that handles basic drive operations
self.leftMotor = wpilib.Talon(1)
self.rightMotor = wpilib.Talon(2)
self.frontSolExtend = wpilib.Solenoid(1, 1)
self.frontSolRetract = wpilib.Solenoid(1, 0)
self.rearSolExtend = wpilib.Solenoid(1, 3)
self.rearSolRetract = wpilib.Solenoid(1, 2)
self.frontSolExtend.set(False)
self.frontSolRetract.set(True)
self.rearSolExtend.set(False)
self.rearSolRetract.set(True)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.stick = wpilib.Joystick(0)
def __init__(self, distance_per_pulse, robot): super().__init__() left_motors_instance = Left_Motors() right_motors_instance = Right_Motors() self.left_motors = left_motors_instance.left_motor_group self.right_motors = right_motors_instance.right_motor_group self.left_shifter = wpilib.Solenoid(0) self.right_shifter = wpilib.Solenoid(1) self.left_encoder = wpilib.Encoder(2,3) self.right_encoder = wpilib.Encoder(4,5) self.left_encoder.setDistancePerPulse(distance_per_pulse) self.right_encoder.setDistancePerPulse(distance_per_pulse) self.gyro = wpilib.ADXRS450_Gyro() self.robot_instance = robot self.drive = DifferentialDrive(self.left_motors, self.right_motors) self.gyro.reset() self.x = 0 self.y = 0 self.heading = math.pi/2 self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0
def __init__(self, port1, port2, _toget, _invert):
self.sol1 = wpilib.Solenoid(port1)
self.sol2 = wpilib.Solenoid(port2)
self.toget = _toget
self.invert = _invert
self.deff = not _invert
self.last = False
self.sets(not self.deff)
def __init__( self, robot ): super( ).__init__( 'gear funnel' ) self.robot = robot self.gear_funnel_solenoid_1 = wpilib.Solenoid( const.ID_PCM_2, const.ID_GEAR_FUNNEL_SOLENOID_1 ) self.gear_funnel_solenoid_2 = wpilib.Solenoid( const.ID_PCM_2, const.ID_GEAR_FUNNEL_SOLENOID_2 ) # Funnel Open means the Funnel to collect the gear is pushed out and the lexan is not in the way self.gear_funnel_state = const.ID_GEAR_FUNNEL_CLOSED
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = XboxController(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.drive_motor_a = CANTalon(2)
self.drive_motor_b = CANTalon(5)
self.drive_motor_c = CANTalon(4)
self.drive_motor_d = CANTalon(3)
self.gear_alignment_motor = CANTalon(14)
self.winch_motor = CANTalon(11)
self.winch_motor.setInverted(True)
self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=0,
reverseChannel=1)
# self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=3,
# reverseChannel=4)
self.gear_push_solenoid = wpilib.Solenoid(2)
self.gear_drop_solenoid = wpilib.Solenoid(3)
# self.gear_push_solenoid = wpilib.DoubleSolenoid(forwardChannel=1, reverseChannel=2)
# self.gear_drop_solenoid = wpilib.Solenoid(0)
self.test_trajectory = generate_trapezoidal_trajectory(
0, 0, 3, 0, Chassis.max_vel, 1, -1, Chassis.motion_profile_freq)
self.throttle = 1.0
self.direction = 1.0
self.led_dio = wpilib.DigitalOutput(1)
self.compressor = wpilib.Compressor()
def robotInit(self):
# A way of demonstrating the difference between the game data strings
self.blue = wpilib.Solenoid(0)
self.red = wpilib.Solenoid(1)
self.green = wpilib.Solenoid(2)
self.yellow = wpilib.Solenoid(3)
# Set game data to empty string by default
self.gameData = ""
# Get the SmartDashboard table from networktables
self.sd = NetworkTables.getTable("SmartDashboard")
def createObjects(self):
# Define Driving Motors
self.rightDrive = wpilib.VictorSP(0)
self.leftDrive = wpilib.VictorSP(1)
# Create Robot Drive
self.robotDrive = wpilib.drive.DifferentialDrive(
self.rightDrive, self.leftDrive)
# Create Shifter Pneumatics
self.shifterSolenoid = wpilib.DoubleSolenoid(0, 0, 1)
# Joysticks and Controllers
self.driveJoystick = wpilib.Joystick(0)
self.driveController = XboxController(0)
self.subsystemJoystick = wpilib.Joystick(1)
self.subsystemController = XboxController(1)
# Create NavX and Accel
self.navX = navx.AHRS.create_spi()
self.accel = wpilib.BuiltInAccelerometer()
# Set Drivespeed
self.driveSpeed = 0
# Intake Motors
self.intakeMotor = wpilib.VictorSP(2)
# Intake Lifter
self.intakeLifter = wpilib.Spark(6)
# Create Cube Intake Pneumatics
self.intakeSolenoid = wpilib.Solenoid(0, 2)
# Create Ramp Motors
self.rightRamp = wpilib.Spark(5)
self.leftRamp = wpilib.Spark(4)
# Create Ramp Deploy Pneumatics
self.rampSolenoid = wpilib.Solenoid(0, 3)
# Create Timer (For Making Timed Events)
self.timer = wpilib.Timer()
# Initialize Compressor
self.compressor = wpilib.Compressor()
# Create CameraServer
wpilib.CameraServer.launch("common/multipleCameras.py:main")
# Set Gear in Dashboard
wpilib.SmartDashboard.putString("Shift State", "Low Gear")
wpilib.SmartDashboard.putString("Cube State", "Unclamped")
def robotInit(self):
self.pop = Camara2.main()
wpilib.CameraServer.launch()
# NetworkTables.startClientTeam(5716)
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize()
self.pc = NetworkTables.getTable("SmartDashboard")
# self.cond = threading.Condition()
# self.notified = [False]
#NetworkTables.initialize(server='roborio-5716-frc.local')
#NetworkTables.initialize()
#self.sd = NetworkTables.getTable('SmartDashboard')
# wpilib.CameraServer.launch()
# cap = cv2.VideoCapture(0)
# self.Video = VideoRecorder()
# wpilib.CameraServer.launch()
# self.chasis_controller = wpilib.Joystick(0)
self.timer = wpilib.Timer()
self.left_cannon_motor = wpilib.Talon(5)
self.right_cannon_motor = wpilib.Talon(6)
self.sucker = wpilib.Talon(7)
self.front_left_motor = wpilib.Talon(3)
self.rear_left_motor = wpilib.Talon(1)
self.front_right_motor = wpilib.Talon(4)
self.rear_right_motor = wpilib.Talon(2)
self.front_left_motor.setInverted(True)
self.color_wheel = wpilib.Talon(8)
self.drive = MecanumDrive(
self.front_left_motor,
self.rear_left_motor,
self.front_right_motor,
self.rear_right_motor)
#led
self.green_led = wpilib.DigitalOutput(0)
#cannon pneumatic
self.Compressor = wpilib.Compressor(0)
self.PSV = self.Compressor.getPressureSwitchValue()
self.cannon_piston = wpilib.Solenoid(0,5)
self.hook1 = wpilib.Solenoid(0,0)
self.hook2 = wpilib.Solenoid(0,7)
def __init__(self, robot):
super().__init__('gear lexan')
self.robot = robot
self.gear_lexan_solenoid_1 = wpilib.Solenoid(
const.ID_PCM_2, const.ID_GEAR_LEXAN_SOLENOID_1)
self.gear_lexan_solenoid_2 = wpilib.Solenoid(
const.ID_PCM_2, const.ID_GEAR_LEXAN_SOLENOID_2)
# Lexan Open means the slider is extended, ready to direct fuel into hopper
self.gear_lexan_state = const.ID_GEAR_LEXAN_CLOSED
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
"a",
steer_talon=ctre.WPI_TalonSRX(48),
drive_talon=ctre.WPI_TalonSRX(49),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.WPI_TalonSRX(46),
drive_talon=ctre.WPI_TalonSRX(47),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.WPI_TalonSRX(44),
drive_talon=ctre.WPI_TalonSRX(45),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.WPI_TalonSRX(42),
drive_talon=ctre.WPI_TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left = ctre.WPI_TalonSRX(0)
self.intake_right = ctre.WPI_TalonSRX(1)
self.clamp_arm = wpilib.Solenoid(0)
self.intake_kicker = wpilib.Solenoid(1)
self.extension_arm_left = wpilib.Solenoid(2)
self.extension_arm_right = wpilib.Solenoid(3)
self.infrared = SharpIRGP2Y0A41SK0F(0)
self.lifter_motor = ctre.WPI_TalonSRX(3)
self.cube_switch = wpilib.DigitalInput(0)
self.center_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
# create the imu object
self.bno055 = BNO055()
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.sd = NetworkTables.getTable("SmartDashboard")
self.spin_rate = 5
def __init__(self, sensors, joystick):
self.openIntake = wpilib.Solenoid(robot_map.OPEN_INTAKE)
self.closeIntake = wpilib.Solenoid(robot_map.CLOSE_INTAKE)
self.rightIntake = wpilib.Talon(robot_map.RIGHT_INTAKE)
self.leftIntake = wpilib.Talon(robot_map.LEFT_INTAKE)
self.sensors = sensors
self.joystick = joystick
self.pdp = wpilib.PowerDistributionPanel()
def __init__(self, robot):
'''
Unfolds climber using two stages of unfolding
'''
super().__init__("climber")
# Subsystem.__init__(self, "climber")
# Note to self: Biggum is two pistons on the lower stage which are
# currently on the same solenoid port
self.biggum = wpilib.Solenoid(1)
# Two pistons for the upper stage on same port
self.littlum = wpilib.Solenoid(2)
def __init__(self):
super().__init__()
'''
Command Dependencies:
Shifter On/Off
Initialize Pneumatics[shifters]
Each solenoid is instantiated by
an "actuated" and "unactuated" command (on and off, respectively)
'''
self.shifter_solenoid_left = wpilib.Solenoid(0)
self.shifter_solenoid_right = wpilib.Solenoid(1)
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 module_init(self):
self.joystick = wpilib.Joystick(0)
self.intake = self.engine.get_module("intake")
self.lift_talon_0 = wpilib.CANTalon(5)
self.lift_talon_0.enableBrakeMode(True)
self.lift_talon_1 = wpilib.CANTalon(6)
self.lift_talon_1.enableBrakeMode(True)
self.lift_solenoid = wpilib.Solenoid(7)
self.lock_solenoid = wpilib.Solenoid(6)
self.lifter_activated = False
self.lock_activated = True
self.unreel_for = 0
def __init__( self, robot ): super( ).__init__( 'ground feeder' ) self.robot = robot #if you feel that this should be 0 and 1, because programmers count from 0 then too bad. self.feeder_motor_1 = wpilib.VictorSP( const.ID_FEEDER_MOTOR_1 ) self.feeder_motor_2 = wpilib.VictorSP( const.ID_FEEDER_MOTOR_2 ) self.feeder_motor_2.setInverted( True ) self.feeder_solenoid_1 = wpilib.Solenoid( const.ID_PCM_1, const.ID_FEEDER_SOLENOID_1 ) self.feeder_solenoid_2 = wpilib.Solenoid( const.ID_PCM_1, const.ID_FEEDER_SOLENOID_2 ) self.feeder_running = const.ID_FEEDER_STOPPED self.feeder_solenoid_state = const.ID_FEEDER_ENGAGED
def __init__(self, robot):
print("[HatchSystem] initializing")
super().__init__("HatchSystem")
self.robot = robot
self.motor = ctre.WPI_TalonSRX(self.SLIDER_MOTOR)
self.led = wpilib.Solenoid(self.PCM_CAN_ID, self.LED_SOLENOID_ID)
self.ejector = wpilib.Solenoid(self.PCM_CAN_ID, self.PISTON_SOLENOID_ID)
self.compressor = wpilib.Compressor(self.COMPRESSOR_PIN)
wpilib.LiveWindow.addActuator("HatchSystem", "Alignment Motor", self.motor)
print("[HatchSystem] initialized")
def __init__(self):
Subsystem.__init__(self, "DropFeeder")
self.DropFeeder1 = wpilib.Solenoid(robot_map.pcm["Dropper1"])
self.DropFeeder2 = wpilib.Solenoid(robot_map.pcm["Dropper2"])
def get(self):
return self.DropFeeder1.get()
return self.DropFeeder2.get()
def set(self, input):
self.Dropper1.set(input)
self.Dropper2.set(input)
def initDefaultCommand(self):
pass
def __init__(self, robot):
self.robot = robot
self.leftArm = Talon(self.robot.kCubeGrabber['left_arm'])
self.rightArm = Talon(self.robot.kCubeGrabber['right_arm'])
self.armUS = wpilib.AnalogInput(self.robot.kCubeGrabber['ultra_sonic'])
self.armSwitch = wpilib.DigitalInput(self.robot.kCubeGrabber['switch'])
self.armClosePosition = self.robot.kCubeGrabber['close']
self.armOpenPosition = self.robot.kCubeGrabber['open']
self.driverTwo = self.robot.cStick
self.armSolenoid = wpilib.Solenoid(self.robot.pneumaticControlModuleCANID, self.robot.kCubeGrabber['solenoid'])
"""
Initializes the predetermined distances for grabber/cube interaction.
"""
self.spinDistance = 12
self.closeDistance = 7
"""
Initializes Toggle Counts for the arm functionality.
"""
self.armModeToggleCount = 2
self.openToggleCount = 3
def solenoidFactory(descp):
"""
Creates single and double solenoids devices from descp
"""
try:
pcm = 0
if pcm in descp:
pcm = descp["pcm"]
if descp["type"] == "solenoid":
return wpilib.Solenoid(pcm, descp["channel"])
if descp["type"] == "doubleSolenoid":
solenoid = wpilib.DoubleSolenoid(pcm, descp["channel"]["forward"],
descp["channel"]["reverse"])
if "default" in descp:
value = {
"kOff": 0,
"kForward": 1,
"kReverse": 2
}[descp["default"]]
solenoid.set(wpilib.DoubleSolenoid.Value(value))
return solenoid
except Exception as e:
logging.error("Failed to create solenoid %s. Err %s", descp, e)
return None
def __init__(self):
super().__init__()
'''
Command Dependencies:
Hatch Panel Intake/Eject
Initialize Pneumatics[hatch panel intake]
Each solenoid is instantiated by
an "actuated" and "unactuated" command
(intake and eject, respectively)
'''
# BEAK
self.beak = wpilib.Solenoid(2)
# 4 BAR
self.bars = wpilib.Solenoid(3)
def createObjects(self):
"""Robot initialization function"""
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.indexer_motors = [ctre.WPI_TalonSRX(3)]
self.indexer_switches = [wpilib.DigitalInput(9)]
self.injector_slave_motor = ctre.WPI_TalonSRX(43)
self.injector_slave_motor.follow(self.indexer_motors[0])
self.injector_slave_motor.setInverted(True)
self.loading_piston = wpilib.Solenoid(0)
self.shooter_centre_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.shooter_outer_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.colour_sensor = rev.color.ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.spinner_motor = wpilib.Spark(2)
self.spinner_solenoid = wpilib.DoubleSolenoid(2, 3)
self.turret_centre_index = wpilib.DigitalInput(1)
self.turret_motor = ctre.WPI_TalonSRX(10)
self.vision = Vision()
# operator interface
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.spinner_joystick = wpilib.Joystick(2)
self.turret_joystick = wpilib.Joystick(3)
def robotInit(self):
'''Robot initialization function - Define your inputs, and what channels they connect to'''
self.robotDrive = wpilib.RobotDrive(self.frontLeftChannel,
self.rearLeftChannel,
self.frontRightChannel,
self.rearRightChannel)
self.robotDrive.setExpiration(0.1)
self.robotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, True)
self.robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, True)
self.winch_motor2 = wpilib.Talon(self.winchMotor2)
self.winch_motor1 = wpilib.Talon(self.winchMotor1)
self.stick = wpilib.Joystick(self.joystickChannel)
self.fire_single_piston = wpilib.buttons.JoystickButton(self.stick, 1)
self.fire_double_forward = wpilib.buttons.JoystickButton(self.stick, 2)
self.fire_double_backward = wpilib.buttons.JoystickButton(self.stick, 3)
self.single_solenoid = wpilib.Solenoid(1)
self.double_solenoid = wpilib.DoubleSolenoid(2,3)
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.imu = navx.AHRS.create_spi(update_rate_hz=50)
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.Solenoid(0)
self.intake_main_motor = ctre.WPI_TalonSRX(18)
self.indexer_motors = [
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
]
self.injector_motor = ctre.WPI_TalonSRX(14)
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(2, 3)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.codriver_gamepad = wpilib.XboxController(1)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
# how long before data times out
self.has_zeroed = False
