def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.leftSpark1 = rev.CANSparkMax(15, rev.MotorType.kBrushless)
self.leftSpark2 = rev.CANSparkMax(14, rev.MotorType.kBrushless)
self.leftSpark3 = rev.CANSparkMax(13, rev.MotorType.kBrushless)
self.rightSpark1 = rev.CANSparkMax(20, rev.MotorType.kBrushless)
self.rightSpark2 = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rightSpark3 = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.leftSparks = wpilib.SpeedControllerGroup(self.leftSpark1,
self.leftSpark2,
self.leftSpark3)
self.rightSparks = wpilib.SpeedControllerGroup(self.rightSpark1,
self.rightSpark2,
self.rightSpark3)
self.leftSpark1.setIdleMode(rev.IdleMode.kBrake)
self.leftSpark2.setIdleMode(rev.IdleMode.kBrake)
self.leftSpark3.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark1.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark2.setIdleMode(rev.IdleMode.kBrake)
self.rightSpark3.setIdleMode(rev.IdleMode.kBrake)
self.openCloseSolenoid = wpilib.DoubleSolenoid(2, 5)
self.inOutSolenoid = wpilib.DoubleSolenoid(3, 4)
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
wpilib.CameraServer.launch()
# LiveWindow slows down the robot, and we aren't using it
wpilib.LiveWindow.disableAllTelemetry()
self.mainStick = wpilib.Joystick(0)
self.extraStick = wpilib.Joystick(1)
self.armUp = False
self.drive_l1 = ctre.WPI_TalonSRX(1)
self.drive_l2 = ctre.VictorSPX(2)
self.drive_l3 = ctre.VictorSPX(3)
self.drive_r1 = ctre.WPI_TalonSRX(4)
self.drive_r2 = ctre.VictorSPX(5)
self.drive_r3 = ctre.VictorSPX(6)
self.elevator_motor1 = ctre.WPI_TalonSRX(7)
self.arm_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.cargo_intake_motor = ctre.WPI_TalonSRX(11)
self.hatch_intake_motor = ctre.WPI_TalonSRX(8)
self.shiftSolenoid1 = wpilib.DoubleSolenoid(0, 1)
self.shiftSolenoid2 = wpilib.DoubleSolenoid(3, 2)
self.blinkin = wpilib.Spark(1)
self.gear = 1
self.irSensor = SharpIR2Y0A21(0)
def robotInit(self): #This function is called upon program startup and should be used for mapping everything
#motors
self.motorL = wpilib.Spark(0) #channel 0
self.motorR = wpilib.Spark(1) #channel 1
self.drive = DifferentialDrive(self.motorL, self.motorR) #dive setup, differential is used with tank
#solenoids
self.arm = wpilib.DoubleSolenoid(0,0,1) #modul 0 channels 0 and 1
self.claw = wpilib.DoubleSolenoid(0,2,3) #modul 0 channels 2 and 3
#controller
self.controller = wpilib.Joystick(1)
#in code use the following for each button or joystick with Logitech in "D" mode
#left joystick horizontal - self.controller.getX()
#left joystick vertical - self.controller.getY()
#right joystick horizontal - self.controller.getZ()
#right joystick vertical - self.controller.getT()
#button X - self.controller.getButton(1)
#button A - self.controller.getButton(2)
#button B - self.controller.getButton(3)
#button Y - self.controller.getButton(4)
#trigger top left - self.controller.getButton(5)
#trigger top right - self.controller.getButton(6)
#bumper bottom left - self.controller.getButton(7)
#bumper bottom right - self.controller.getButton(8)
#button Back - self.controller.getButton(9)
#button Start - self.controller.getButton(10)
self.timer = wpilib.Timer()
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(3)
self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(5)
self.frontRightMotor = wpilib.Spark(0)
self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(2)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.middleLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.middleRightMotor, self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = 'LRL'
# joysticks 1 & 2 on the driver station
self.Stick1 = wpilib.XboxController(0)
self.Stick2 = wpilib.Joystick(1)
self.aSolenoidLow = wpilib.DoubleSolenoid(2,3)
self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1)
self.iSolenoid = wpilib.DoubleSolenoid(4,5)
def robotInit(self):
# assign motors to object
self.motorLeftFront = ctre.WPI_TalonSRX(Robot.frontLeftPort)
self.motorLeftBack = ctre.WPI_TalonSRX(Robot.backLeftPort)
self.motorRightFront = ctre.WPI_TalonSRX(Robot.frontRightPort)
self.motorRightBack = ctre.WPI_TalonSRX(Robot.backRightPort)
# invert motors
self.motorLeftFront.setInverted(True)
self.motorLeftBack.setInverted(True)
# make motor groups
self.leftMotors = wpilib.SpeedControllerGroup(self.motorLeftBack,
self.motorLeftFront)
self.rightMotors = wpilib.SpeedControllerGroup(self.motorRightBack,
self.motorRightFront)
# create a drivetrain ovject to access motors easier
self.drivetrain = wpilib.drive.DifferentialDrive(
self.leftMotors, self.rightMotors)
# set up a timer to allow for cheap drive by time auto
self.timer = wpilib.Timer()
# initialize OI systems for the robot
self.OI = OI()
# solenoids
self.leftSolenoid = wpilib.DoubleSolenoid(Robot.leftSolenoidIn,
Robot.leftSolenoidOut)
self.rightSolenoid = wpilib.DoubleSolenoid(Robot.rightSolenoidIn,
Robot.rightSolenoidOut)
def __init__(self, my_robot):
super().__init__(my_robot)
self.grab = wpilib.DoubleSolenoid(0, 1)
self.lift = wpilib.DoubleSolenoid(2, 3)
self.arm_up_last = True
self.grab_close_last = True
def robotInit(self):
"""This function initiates the robot's components and parts"""
# Here we create a function for the command class to return the robot
# instance, so that we don't have to import the robot module for each
# command.
Command.getRobot = lambda _: self
# This launches the camera server between the robot and the computer
wpilib.CameraServer.launch()
self.joystick = wpilib.Joystick(0)
self.lr_motor = ctre.WPI_TalonSRX(1)
self.lf_motor = ctre.WPI_TalonSRX(2)
self.rr_motor = ctre.WPI_TalonSRX(5)
self.rf_motor = ctre.WPI_TalonSRX(6)
self.left = wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor)
self.right = wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)
self.drivetrain_solenoid = wpilib.DoubleSolenoid(2, 3)
self.drivetrain_gyro = wpilib.AnalogGyro(1)
# Here we create the drivetrain as a whole, combining all the different
# robot drivetrain compontents.
self.drivetrain = drivetrain.Drivetrain(self.left, self.right,
self.drivetrain_solenoid,
self.drivetrain_gyro,
self.rf_motor)
self.l_gripper = wpilib.VictorSP(0)
self.r_gripper = wpilib.VictorSP(1)
self.grippers = grippers.Grippers(self.l_gripper, self.r_gripper)
self.elevator_motor = wpilib.VictorSP(2)
self.elevator_top_switch = wpilib.DigitalInput(4)
self.elevator_bot_switch = wpilib.DigitalInput(5)
self.elevator = elevator.Elevator(self.elevator_motor,
self.elevator_top_switch,
self.elevator_bot_switch)
self.handles_solenoid = wpilib.DoubleSolenoid(0, 1)
self.handles = handles.Handles(self.handles_solenoid)
# This creates the instance of the autonomous program that will run
# once the autonomous period begins.
self.autonomous = AutonomousProgram()
# This gets the instance of the joystick with the button function
# programmed in.
self.josytick = oi.getJoystick()
def module_init(self):
self.intake_solenoid_1 = wpilib.DoubleSolenoid(0, 1)
self.intake_solenoid_2 = wpilib.DoubleSolenoid(2, 3)
if self.USE_CAN:
self.intake_motor = wpilib.CANTalon(6)
self.intake_motor.enableControl()
else:
self.intake_motor = wpilib.Talon(7)
self.joystick = wpilib.Joystick(1)
def __init__(self):
print('TankLift: init called')
super().__init__('TankLift')
self.logPrefix = "TankLift: "
self.frontCylinder = wpilib.DoubleSolenoid(1,0,1) # 1st arg= CAN ID=1, then takes ports on pcm to energize solenoid
self.backCylinder = wpilib.DoubleSolenoid(1,2,3) # 1st arg= CAN ID=1, ...
self.frontIR = wpilib.DigitalInput(robotmap.driveLine.frontIRPort)
self.backIR = wpilib.DigitalInput(robotmap.driveLine.backIRPort)
def robotInit(self):
#Controllers
self.driver = wpilib.XboxController(0)
self.operator = wpilib.XboxController(1)
#Motors
self.left_motor_1 = rev.CANSparkMax(robotmap.LEFT_LEADER_ID, rev.MotorType.kBrushed)
self.left_motor_2 = rev.CANSparkMax(robotmap.LEFT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.right_motor_1 = rev.CANSparkMax(robotmap.RIGHT_LEADER_ID, rev.MotorType.kBrushed)
self.right_motor_2 = rev.CANSparkMax(robotmap.RIGHT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.left_motor_1.setClosedLoopRampRate(1.0)
self.left_motor_2.setClosedLoopRampRate(1.0)
self.right_motor_1.setClosedLoopRampRate(1.0)
self.right_motor_2.setClosedLoopRampRate(1.0)
self.left_side = wpilib.SpeedControllerGroup(self.left_motor_1, self.left_motor_2)
self.right_side = wpilib.SpeedControllerGroup(self.right_motor_1, self.right_motor_2)
#Drivetrain
self.drivetrain = wpilib.drive.DifferentialDrive(self.left_side, self.right_side)
#TODO: Add subsystems and sensors as the code is written
#TODO: SmartDashboard
#Pneumatics
self.colorPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.COLOR_SENSOR_EXTEND, robotmap.COLOR_SENSOR_RETRACT))
self.climberPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.CLIMBER_EXTEND, robotmap.CLIMBER_RETRACT))
self.gearshiftPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.DRIVE_SHIFT_EXTEND, robotmap.DRIVE_SHIFT_RETRACT))
self.climberWinchMotor1 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR1, rev.MotorType.kBrushed)
self.climberWinchMotor2 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR2, rev.MotorType.kBrushed)
# Color Sensor
self.colorSensor = color_sensor()
self.colorSensorMotor = rev.CANSparkMax(robotmap.COLOR_SENSOR_MOTOR, rev.MotorType.kBrushed)
self.stopColorMap = {"R":"B", "Y":"G", "B":"R", "G":"Y"}
self.gameData = ""
self.setupColorSensor()
#Shooter
self.shooter = shooter(robotmap.LOADER, robotmap.SHOOTER)
# Gyro
self.ahrs = AHRS.create_spi()
self.Aimer = Aimer(self.ahrs)
#network tables
self.sd = NetworkTables.getTable('VISION')
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotInit(self):
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(
1) # Initialize the TalonSRX on device 1.
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.motor5 = ctre.WPI_TalonFX(5) #intake Motor
self.motor6 = ctre.WPI_TalonFX(6) #Shooter Motor
self.motor7 = ctre.WPI_VictorSPX(7) #Intake Arm
self.motor8 = ctre.WPI_VictorSPX(8) #Belt Drive
self.joy = wpilib.Joystick(0)
self.stick = wpilib.Joystick(
1) #this is a controller, also acceptable to use Joystick
self.intake = wpilib.DoubleSolenoid(0, 1)
self.balls = wpilib.DoubleSolenoid(2, 3)
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
#This is combining the Speed controls from above to make a left and right
#for the drive chassis. Note that self.motor1 and self.motor2 are combined to make self.left
#then self.motor3 and self.motor4 are combined to make self.right. This is done to Make
#the differential drive easier to setup
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
#Here is our DifferentialDrive, Ultimately stating, Left side and Right side of chassis
#An Alternative to DifferentialDrive is this:
#self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2)
#where motor 1 & 4 are left, and 2 & 3 are right
self.myRobot.setExpiration(0.1)
#These components here are for Autonomous Modes, and allows you to call parts and
#components here to be used IN automodes. For example- Our chassis above is labeled
#'self.myRobot', below in the self.components, If we want to use our chassis to drive
#in Autonomous, we need to call it in the fashion below. It is also encouraged to
#reuse the naming of the components from above to avoid confusion below. i.e.
#'Chassis': self.myRobot, In autonomous creation, I would then be using the variable
#self.chassis.set() instead of self.myRobot.set() when doing commands.
self.components = {
'myRobot': self.myRobot, #Chassis Driving
'motor5': self.motor5,
'motor6': self.motor6, #A speed control that is used for intake
'intake': self.intake,
'balls': self.balls #pneumatics arm that is not setup on bot yet
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def createObjects(self):
# TEMP - PRACTICE BOT ONLY
# Launch cameraserver
# wpilib.CameraServer.launch()
# Practice bot
# On practice bot, DIO is shorted
# self.is_practice_bot = wpilib.DigitalInput(30)
if hal.HALIsSimulation():
self.is_practice_bot = wpilib.DigitalInput(20)
else:
self.is_practice_bot = wpilib.DigitalInput(30)
# Drivetrain
self.drivetrain_left_motor_master = ctre.WPI_TalonSRX(4)
self.drivetrain_left_motor_slave = ctre.WPI_TalonSRX(7) # was 3
self.drivetrain_right_motor_master = ctre.WPI_TalonSRX(5)
self.drivetrain_right_motor_slave = ctre.WPI_TalonSRX(6)
self.drivetrain_shifter_solenoid = wpilib.Solenoid(0)
# Elevator
self.elevator_motor = ctre.WPI_TalonSRX(8)
self.elevator_solenoid = wpilib.DoubleSolenoid(1, 2)
self.elevator_reverse_limit = wpilib.DigitalInput(0)
# Grabber
self.grabber_left_motor = ctre.WPI_TalonSRX(1)
self.grabber_right_motor = ctre.WPI_TalonSRX(2)
# Ramp
self.ramp_solenoid = wpilib.DoubleSolenoid(3, 4)
self.ramp_motor = ctre.WPI_TalonSRX(3) # was 7
self.hold_start_time = None
# Controllers
self.drive_controller = wpilib.XboxController(0)
self.operator_controller = wpilib.XboxController(1)
# Compressor
self.compressor = wpilib.Compressor()
# LEDs
self.led_driver = wpilib.Spark(0)
# Navx
self.navx = navx.AHRS.create_spi()
# Macros / path recorder
self._is_recording_macro = False
self._is_playing_macro = False
self._is_recording_path = False
# Flippy toggle
self._is_flippy = False
def robotInit(self):
# Robot initialization function
# VictorSPX = Motor Controllers
self.frontLeftMotor = ctre.WPI_VictorSPX(0)
self.rearLeftMotor = ctre.WPI_VictorSPX(1)
self.frontRightMotor = ctre.WPI_VictorSPX(4)
self.rearRightMotor = ctre.WPI_VictorSPX(5)
self.basketMotor = ctre.WPI_VictorSPX(3)
# Digital Inputs (Limit Switch)
self.basketLimitSwitch = wpilib.DigitalInput(0)
# Motor controller groups for each side of the robot
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# Differential drive with left and right motor controller groups
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.direction = -1
# Declare gamepad
self.gamepad = wpilib.Joystick(0)
# Declare buttons
# Controller mapping (1-10): B (East), A (South), Y (North), X (West), Right Bumper, Left Bumper, ?, ?, ?, ?
self.toggleHatchButton = JoystickButton(self.gamepad, 1)
self.toggleBasketButton = JoystickButton(self.gamepad, 2)
self.toggleDirectionButton = JoystickButton(self.gamepad, 3)
self.speedUpButton = JoystickButton(self.gamepad, 4)
self.raiseBasketButton = JoystickButton(self.gamepad, 5)
self.lowerBasketButton = JoystickButton(self.gamepad, 6)
# Determine if already acted on
self.hatchActed = False
self.basketActed = False
self.directionActed = False
# Solenoids
self.hatchSolenoid = wpilib.DoubleSolenoid(0, 1)
self.basketSolenoid = wpilib.DoubleSolenoid(2, 3)
# Compressor
self.compressor = wpilib.Compressor(0)
# Camera Server
wpilib.CameraServer.launch()
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 createObjects(self):
self.stager_used = False
# self.pdp = wpilib.PowerDistributionPanel()
self.reverse_stager_used = False
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.ll = NetworkTables.getTable("limelight")
self.controlPanel = wpilib.Joystick(5)
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
self.climbMotor = wpilib.Victor(2)
self.stagerMotor = wpilib.Victor(3)
self.frontShooterMotor = wpilib.Victor(9)
self.elevatorMotor = wpilib.Victor(6)
self.elevatorMotor.setInverted(True)
self.shooterTiltMotor = wpilib.Victor(7)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
self.climbMotor.setInverted(True)
self.stagerMotor.setInverted(True)
self.punchers = wpilib.DoubleSolenoid(0, 0, 1)
self.skis = wpilib.DoubleSolenoid(4, 5)
self.launcherRotate = wpilib.AnalogInput(0)
self.climbLimitSwitch = wpilib.DigitalInput(8)
self.climb_raise_limitswitch = wpilib.DigitalInput(4)
self.ball_center = wpilib.DigitalInput(9)
self.elevator_limit = wpilib.DigitalInput(7)
self.pins = wpilib.DoubleSolenoid(2,3)
self.pins.set(2)
self.tilt_limit = wpilib.DigitalInput(6)
self.tilt_controller = wpilib.PIDController(2,0,0, self.launcherRotate, self.shooterTiltMotor)
# #Practice bot(2,0,0)
# #Comp bot(3.66,0,0)
self.tilt_controller.setOutputRange(-1,.5)
self.tilt_controller.setPercentTolerance(5)
self.elevator_encoder = wpilib.Encoder(0, 1)
self.elevator_controller = wpilib.PIDController(.0025, 0, .001, self.elevator_encoder, self.elevatorMotor)
# #practice bot(0.008,0,0.005)
# #Comp bot(.0025,0,.001)
self.elevator_controller.setOutputRange(-1,.44)
self.elevator_controller.setPercentTolerance(10)
self.gears = wpilib.DoubleSolenoid(6,7)
self.tilt_disabled = True
self.punchers.set(2)
self.skis.set(2)
self.oldtx = 0
self.gears.set(1)
self.tilt_disabled = True
self.controlPanel.setOutputs(False)
self.color_sensor_left = wpilib.DigitalInput(3)
self.color_sensor_mid = wpilib.DigitalInput(5)
self.color_sensor_right = wpilib.DigitalInput(2)
self.ultra = wpilib.AnalogInput(1)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.table = NetworkTables.getTable("SmartDashboard")
self.robot_drive = wpilib.drive.DifferentialDrive(
wpilib.Spark(0), wpilib.Spark(1))
self.stick = wpilib.Joystick(0)
self.elevatorMotor = wpilib.VictorSP(5)
self.intakeMotor = wpilib.VictorSP(2)
self.intakeMotorLeft = wpilib.VictorSP(3)
self.intakeMotorRight = wpilib.VictorSP(4)
self.climbMotor = wpilib.VictorSP(6)
self.ahrs = AHRS.create_i2c(0)
#self.gearSpeed = .5
#self.lights = wpilib.Relay(1)
#self.lightToggle = False
#self.lightToggleBool = True
#self.togglev = 0
self.robot_drive.setSafetyEnabled(False)
wpilib.CameraServer.launch()
self.xb = wpilib.Joystick(1)
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enabled = self.Compressor.enabled()
self.PSV = self.Compressor.getPressureSwitchValue()
self.LeftSolenoid = wpilib.DoubleSolenoid(0, 1)
self.RightSolenoid = wpilib.DoubleSolenoid(2, 3)
self.Compressor.start()
self.wheel = wpilib.Encoder(0, 1)
self.wheel2 = wpilib.Encoder(2, 3, True)
self.encoder = Sensors.Encode(self.wheel, self.wheel2)
#wpilib.CameraServer.launch()
self.ultrasonic = wpilib.AnalogInput(0)
self.autoSchedule = Auto.Auto(self, )
self.intakeToggle = False
self.intakePos = False
self.openSwitch = wpilib.DigitalInput(9)
self.closedSwitch = wpilib.DigitalInput(8)
self.speed = 0.6
self.leftSpeed = 0.7
self.rightSpeed = 0.7
self.speedToggle = False
def __init__(self):
print('TankLift: init called')
super().__init__('TankLift')
self.logPrefix = "TankLift: "
try:
self.frontCylinder = wpilib.DoubleSolenoid(
1, robotmap.driveLine.solenoidRetractFrontPort,
robotmap.driveLine.solenoidExtendFrontPort
) # 1st arg= CAN ID=1, then takes ports on pcm to energize solenoid
except Exception as e:
print("{}Exception caught instantiating front lift cylinder. {}".
format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.backCylinder = wpilib.DoubleSolenoid(
1, robotmap.driveLine.solenoidRetractBackPort,
robotmap.driveLine.solenoidExtendBackPort)
except Exception as e:
print("{}Exception caught instantiating back lift cylinder. {}".
format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.frontIR = wpilib.AnalogInput(robotmap.driveLine.frontIRPort)
except Exception as e:
print(
"{}Exception caught instantiating front IR sensor. {}".format(
self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.backIR = wpilib.AnalogInput(robotmap.driveLine.backIRPort)
except Exception as e:
print("{}Exception caught instantiating back IR sensor. {}".format(
self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
self.allSolToggle = False
self.frontSolToggle = False
self.backSolToggle = False
self.backIR.state = True
self.frontIR.state = True
def module_init(self):
self.intake_solenoid_1 = wpilib.DoubleSolenoid(0, 1)
if self.DOUBLE_SOLENOID:
self.intake_solenoid_2 = wpilib.DoubleSolenoid(2, 3)
if self.USE_CAN:
self.intake_motor = wpilib.CANTalon(6)
self.intake_motor.enableControl()
else:
self.intake_motor = wpilib.Talon(9)
self.auto_override = False
self.intake_potentiometer = wpilib.AnalogPotentiometer(0)
self.setpoint = 0
self.at_setpoint = 0
self.auto_intake_out = 0
self.joystick = wpilib.Joystick(1)
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 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__()
self.joystick = wpilib.XboxController(0)
self.left_drive_motor = wpilib.Talon(0)
self.left_drive_motor_2 = wpilib.Talon(1)
self.right_drive_motor = wpilib.Talon(2)
self.right_drive_motor_2 = wpilib.Talon(3)
self.left_drive_motor_group = wpilib.SpeedControllerGroup(
self.left_drive_motor, self.left_drive_motor_2)
self.right_drive_motor_group = wpilib.SpeedControllerGroup(
self.right_drive_motor, self.right_drive_motor_2)
self.drive = DifferentialDrive(self.left_drive_motor_group,
self.right_drive_motor_group)
self.drive_rev = False
self.lift_motor = wpilib.Spark(4)
self.lift_motor_2 = wpilib.Spark(5)
self.lift_motor_group = wpilib.SpeedControllerGroup(
self.lift_motor, self.lift_motor_2)
self.lift_motor_speed = 0
self.lock_controls = False
self.front_motor = wpilib.Spark(6)
self.front_motor_2 = wpilib.Spark(7)
self.front_motor_2.setInverted(True)
self.front_motor_group = wpilib.SpeedControllerGroup(
self.front_motor, self.front_motor_2)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.encoder = wpilib.Encoder(aChannel=0, bChannel=1)
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name="blocker")
self.robot = robot
self.piston = wpilib.DoubleSolenoid(0, 6, 7)
def __init__(self):
'''Instantiates the motor object.'''
super().__init__('SingleMotor')
self.frontLeftMotor = WPI_TalonSRX(channels.frontLeftChannel)
self.rearLeftMotor = WPI_TalonSRX(channels.rearLeftChannel)
self.frontRightMotor = WPI_TalonSRX(channels.frontRightChannel)
self.rearRightMotor = WPI_TalonSRX(channels.rearRightChannel)
self.crossbow = wpilib.DoubleSolenoid(0, 1)
self.crossbow.set(wpilib.DoubleSolenoid.Value.kOff)
self.frontLeftMotor.setInverted(False)
# self.rearLeftMotor.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 30)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def robotInit(self):
"""
Define all motor controllers, joysticks, Pneumatics, etc. here so you can use them in teleop/auton
"""
self.robotDrive = wpilib.RobotDrive(self.topLeftChannel,
self.bottomLeftChannel,
self.topRightChannel,
self.bottomRightChannel)
self.stick = wpilib.Joystick(self.joystickChannelDrive)
self.shooter = wpilib.Joystick(self.joystickChannelShoot)
self.intakeLeft = wpilib.Talon(self.leftIntakeMotor)
self.intakeRight = wpilib.Talon(self.rightIntakeMotor)
self.stage2Left = wpilib.Talon(self.stage2LeftMotor)
self.stage2Right = wpilib.Talon(self.stage2RightMotor)
self.stage3Left = wpilib.Talon(self.stage3LeftMotor)
self.stage3Right = wpilib.Talon(self.stage3RightMotor)
self.sweeper = wpilib.Talon(self.sweeperMotor)
self.shifterForward = wpilib.buttons.JoystickButton(
self.stick, 2) # Xbox controller button Number 2 (B)
self.shifterBack = wpilib.buttons.JoystickButton(
self.stick, 3) # Xbox controller button Number 3 (X)
self.double_solenoid_piston = wpilib.DoubleSolenoid(
2, 3) # Double Solenoid on port 2 and 3
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Drivetrain
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
#joystick
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
#solenoids
self.pinion = wpilib.DoubleSolenoid(0, 1)
self.doubleSolenoid = wpilib.DoubleSolenoid(2, 3)
def __init__(self, sparkNum, pistonNum1, pistonNum2):
self.piston = wpilib.DoubleSolenoid(pistonNum1, pistonNum2)
self.motor = sea.createSpark(sparkNum,
rev.CANSparkMax.MotorType.kBrushless)
self.encoder = self.motor.getEncoder()
self.motor.setIdleMode(rev.CANSparkMax.IdleMode.kCoast)
self.motorController = self.motor.getPIDController()
p = 0.00007
i = 0.0000007
d = 0
f = 0
self.motorController.setP(p)
self.motorController.setI(i)
self.motorController.setD(d)
self.motorController.setFF(f)
self.running = False
self.reversed = False
self.deployed = False
self.stop()
self.retract()
def __init__(self, intake_motor_channel, right_piston_module_number,
right_piston_forward, right_piston_reverse,
left_piston_module_number, left_piston_forward,
left_piston_reverse):
self.intake_motor = WPI_TalonSRX(intake_motor_channel)
self.right_piston = wpilib.DoubleSolenoid(right_piston_module_number,
right_piston_forward,
right_piston_reverse)
self.left_piston = wpilib.DoubleSolenoid(left_piston_module_number,
left_piston_forward,
left_piston_reverse)
self.right_piston_target = self.right_piston.Value.kReverse
self.left_piston_target = self.left_piston.Value.kReverse
self.collect_speed = 0
def __init__(self):
print("Ramp: init called")
super().__init__('Ramp')
self.logPrefix = 'Ramp: '
self.rampSolenoid = wpilib.DoubleSolenoid(1,
robotmap.ramp.solenoidPort1,
robotmap.ramp.solenoidPort2)
def createObjects(self):
with open("../ports.json" if os.getcwd()[-5:-1] == "test" else "ports.json") as f:
self.ports = json.load(f)
with open("../buttons.json" if os.getcwd()[-5:-1] == "test" else "buttons.json") as f:
self.buttons = json.load(f)
# Arm
arm_ports = self.ports["arm"]
self.arm_left = wpilib.Victor(arm_ports["arm_left"])
self.arm_right = ctre.WPI_TalonSRX(arm_ports["arm_right"])
self.wrist = ctre.WPI_TalonSRX(arm_ports["wrist"])
self.intake = wpilib.Spark(arm_ports["intake"])
self.hatch = wpilib.DoubleSolenoid(arm_ports["hatch_in"], arm_ports["hatch_out"])
# DriveTrain
drive_ports = self.ports["drive"]
self.front_left = wpilib.Victor(drive_ports["front_left"])
self.front_right = wpilib.Victor(drive_ports["front_right"])
self.back_left = wpilib.Victor(drive_ports["back_left"])
self.back_right = wpilib.Victor(drive_ports["back_right"])
self.joystick = wpilib.Joystick(0)
self.print_timer = wpilib.Timer()
self.print_timer.start()
self.logger = logging.getLogger("Robot")
self.test_tab = Shuffleboard.getTab("Test")
wpilib.CameraServer.launch()
