def setup_for_robot(robot):
robot.sensor_1 = wpilib.DigitalInput(1)
robot.sensor_2 = wpilib.DigitalInput(2)
robot.sensor_3 = wpilib.DigitalInput(3)
robot.sensor_4 = wpilib.DigitalInput(4)
robot.sensor_5 = wpilib.DigitalInput(5)
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(0)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.left_encoder = wpilib.Encoder(1, 2)
self.right_encoder = wpilib.Encoder(3, 4)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.chassis_speeds = ChassisSpeeds()
self.chassis_speeds.vx = 0.0
self.chassis_speeds.omega = 0.0
if is_sim:
self.physics = physics.PhysicsEngine()
self.last_tm = time.time()
def robotInit(self):
#motores
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
self.lift_motor = wpilib.Talon(4)
self.cargo_motor = wpilib.Talon(5)
#sensores
self.sensor_izquierdo = wpilib.DigitalInput(1)
self.sensor_principal = wpilib.DigitalInput(2)
self.sensor_derecho = wpilib.DigitalInput(3)
#invertidores de motores
self.frontLeftMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
#Unión de los motores para su funcionamiento
# en conjunto de mecaunm
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
def __init__(self, robot):
super().__init__(7.0, 0.0, 8.0, name="Pivot")
self.robot = robot
self.setAbsoluteTolerance(0.005)
self.getPIDController().setContinuous(False)
if robot.isSimulation():
# PID is different in simulation.
self.getPIDController().setPID(0.5, 0.001, 2)
self.setAbsoluteTolerance(5)
# Motor to move the pivot
self.motor = wpilib.Victor(5)
# Sensors for measuring the position of the pivot.
self.upperLimitSwitch = wpilib.DigitalInput(13)
self.lowerLimitSwitch = wpilib.DigitalInput(12)
# 0 degrees is vertical facing up.
# Angle increases the more forward the pivot goes.
self.pot = wpilib.AnalogPotentiometer(1)
# Put everything to the LiveWindow for testing.
wpilib.LiveWindow.addSensor("Pivot", "Upper Limit Switch",
self.upperLimitSwitch)
wpilib.LiveWindow.addSensor("Pivot", "Lower Limit Switch",
self.lowerLimitSwitch)
wpilib.LiveWindow.addSensor("Pivot", "Pot", self.pot)
wpilib.LiveWindow.addActuator("Pivot", "Motor", self.motor)
wpilib.LiveWindow.addActuator("Pivot", "PIDSubsystem Controller",
self.getPIDController())
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.l_motor = wpilib.Jaguar(1)
self.r_motor = wpilib.Jaguar(2)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.motor = wpilib.Jaguar(4)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.leftEncoder = wpilib.Encoder(0, 1)
self.leftEncoder.setDistancePerPulse(
(self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
self.leftEncoder.setSimDevice(0)
self.rightEncoder = wpilib.Encoder(3, 4)
self.rightEncoder.setDistancePerPulse(
(self.WHEEL_DIAMETER) / self.ENCODER_COUNTS_PER_REV)
def setup_for_robot(robot):
robot.sensor_1_line = wpilib.DigitalInput(1)
robot.sensor_2_line = wpilib.DigitalInput(2)
robot.sensor_3_line = wpilib.DigitalInput(3)
robot.sensor_4_line = wpilib.DigitalInput(4)
robot.sensor_5_line = wpilib.DigitalInput(5)
def __init__(self, dio1: ChannelMode, dio2: ChannelMode) -> None:
"""Constructor.
:param dio1: Mode for DIO 1 (input = Button B, output = green LED)
:param dio2: Mode for DIO 2 (input = Button C, output = red LED)
"""
super().__init__()
self.nextMessageTime = 0
self.buttonA = wpilib.DigitalInput(0)
self.buttonB: typing.Optional[wpilib.DigitalInput] = None
self.buttonC: typing.Optional[wpilib.DigitalInput] = None
self.yellowLed = wpilib.DigitalOutput(3)
self.greenLed: typing.Optional[wpilib.DigitalOutput] = None
self.redLed: typing.Optional[wpilib.DigitalOutput] = None
if dio1 == ChannelMode.INPUT:
self.buttonB = wpilib.DigitalInput(1)
else:
self.greenLed = wpilib.DigitalOutput(1)
if dio2 == ChannelMode.INPUT:
self.buttonC = wpilib.DigitalInput(2)
else:
self.redLed = wpilib.DigitalOutput(2)
def __init__(self):
# Mapping object stores port numbers for all connected motors, sensors, and joysticks. See map.py.
Mapping = Map()
# Init drivetrain
self.driveTrain = [wpilib.Spark(Mapping.frontLeftM),
wpilib.Spark(Mapping.frontRightM),
wpilib.Spark(Mapping.backLeftM),
wpilib.Spark(Mapping.backRightM)]
self.driveTrain[0].setInverted(True)
self.driveTrain[2].setInverted(True)
# Init motors
self.elevatorM = wpilib.Spark(Mapping.elevatorM)
self.elevatorM.setInverted(True)
self.winchM = wpilib.Spark(Mapping.winchM)
self.intakeM = wpilib.Spark(Mapping.intakeM)
self.jawsM = wpilib.Spark(Mapping.jawsM)
# Soleniods
self.jawsSol = wpilib.DoubleSolenoid(Mapping.jawsSol['out'], Mapping.jawsSol['in'])
# Init sensors
self.gyroS = wpilib.AnalogGyro(Mapping.gyroS)
self.elevatorLimitS = wpilib.DigitalInput(Mapping.elevatorLimitS)
self.jawsLimitS = wpilib.DigitalInput(Mapping.jawsLimitS)
self.metaboxLimitS = wpilib.DigitalInput(Mapping.metaboxLimitS)
# Encoders
self.elevatorEncoderS = wpilib.Encoder(7, 8, True)
self.elevatorEncoderS.setDistancePerPulse(0.08078)
self.driveYEncoderS = wpilib.Encoder(2, 3)
self.driveYEncoderS.setDistancePerPulse(0.015708)
def __init__(self, name: str, legMotor: BaseMotorController,
extendedId: int, retractedId: int, floorId: int):
"""
Construct new Leg instance.
: param name : Generic name for leg (like "Front").
: param legMotor : The motor controller to use for the leg.
: param extendedId : DIO port ID for sensor at fully extended position.
: param retractedId : DIO port ID for sensor at fully retracted position.
: param floorId : Analog port ID for floor sensor by leg.
"""
self.name = name
self.floorSensor = wpilib.AnalogInput(floorId)
self.floorSensor.setName(group, name + " Floor Sensor")
initializeMotorController(legMotor)
legMotor.setName(group, name + " Leg Motor")
legMotor.setInverted(True)
legMotor.configContinuousCurrentLimit(20, timeout)
legMotor.enableCurrentLimit(True)
legMotor.configVoltageCompSaturation(9, timeout)
legMotor.enableVoltageCompensation(True)
self.legMotor = legMotor
self.retractedSensor = wpilib.DigitalInput(retractedId)
self.retractedSensor.setName(group, name + " Leg Retracted")
self.retractedCounter = wpilib.Counter(self.retractedSensor)
self.retractedCounter.setName(group, name + " Retract Count")
self.extendedSensor = wpilib.DigitalInput(extendedId)
self.extendedSensor.setName(group, name + " Leg Extended")
self.extendedCounter = wpilib.Counter(self.extendedSensor)
self.extendedCounter.setName(group, name + " Extend Count")
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): #Velcrolastick self.lift_motor1 = wpilib.Spark(6) self.lift_motor2 = wpilib.Spark(7) self.sensor1 = wpilib.DigitalInput(7) self.sensor2 = wpilib.DigitalInput(8) # Ismafeeder self.cargo_motor = wpilib.Spark(4) self.lift_motor = wpilib.Spark(5) # Mecanum drive self.frontLeftMotor = wpilib.Talon(0) self.rearLeftMotor = wpilib.Talon(1) self.frontRightMotor = wpilib.Talon(2) self.rearRightMotor = wpilib.Talon(3) self.sensor_1_mec = wpilib.DigitalInput(9) self.frontLeftMotor.setInverted(True) self.rearLeftMotor.setInverted(True) self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor)
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 __init__(self, tote_motor, can_motor):
self.sd = NetworkTable.getTable('SmartDashboard')
self.toteLimitLSensor = wpilib.DigitalInput(0) ##Left limit switch
self.toteLimitRSensor = wpilib.DigitalInput(1) ##Right limit switch
self.longDistanceLSensor = SharpIR2Y0A02(1) # # Robot's left
self.longDistanceRSensor = SharpIR2Y0A02(3) # # Robot's right
self.shortDistanceLSensor = SharpIRGP2Y0A41SK0F(2) # # Robot's left
self.shortDistanceRSensor = SharpIRGP2Y0A41SK0F(7) # # Robot's right
self.leftSensor = CombinedSensor(self.longDistanceLSensor, 22, self.shortDistanceLSensor, 6)
self.rightSensor = CombinedSensor(self.longDistanceRSensor, 22, self.shortDistanceRSensor, 6)
self.tote_motor = tote_motor
self.can_motor = can_motor
self.in_range = False
self.in_range_start = None
# Premature optimization, but it looks nicer
self._tote_exclude_range = set()
# measured using the calibration routine
interference = [(1031, 1387), (1888, 2153), (4544, 4895), (5395, 5664), (8008, 8450)]
#for i in [1033, 2031, 4554, 5393, 7902]:
for r1, r2 in interference:
for j in range(r1, r2):
self._tote_exclude_range.add(j)
self.update()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Initialize Networktables
self.sd = NetworkTables.getTable('SmartDashboard')
#Set up motors to drive robot
self.M2 = wpilib.VictorSP(2)
self.M3 = wpilib.VictorSP(3)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M2, self.M3)
self.M0 = wpilib.VictorSP(0)
self.M1 = wpilib.VictorSP(1)
self.right = wpilib.SpeedControllerGroup(self.M0, self.M1)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
#Camera
wpilib.CameraServer.launch()
#Servo
self.SV1 = wpilib.Servo(9)
self.SV2 = wpilib.Servo(8)
#Dashboard
NetworkTables.initialize(server='10.61.62.2')
#Switches
self.SW0 = wpilib.DigitalInput(0)
self.SW1 = wpilib.DigitalInput(1)
#Elevator
self.E = wpilib.VictorSP(5)
self.prepareCubeFlag = 0
self.grabCubeFlag = 0
self.deliverCubeFlag = 0
self.adjustLeftFlag = 0
self.adjustRightFlag = 0
self.driveFlag = 0
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.reset()
#All possible autonomous routines in a sendable chooser
'''
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("None", '4')
self.chooser.addObject("left-LeftScale", '1')
self.chooser.addObject("Middle-LeftScale", '2')
self.chooser.addObject("Right-LeftScale", '3')
self.chooser.addObject("Left-RightScale", '5')
'''
#wpilib.SmartDashboard.putData('Choice', self.chooser)
#Encoders
self.EC1 = wpilib.Encoder(2, 3)
self.EC2 = wpilib.Encoder(4, 5)
self.EC1.reset()
self.EC2.reset()
def teleopInit(self):
self.compressor = wpilib.compressor
# self.compressor.setClosedLoopControl(True)
self.dsolenoid = wpilib.doublesolenoid(1, 2)
self.limitSwitch = wpilib.DigitalInput(0)
self.limitSwitch2 = wpilib.DigitalInput(1)
def __init__(self):
self.armMotor = ctre.wpi_talonsrx.WPI_TalonSRX(armMotor)
self.armTimer = Timer()
self.bottomStopSwitch = wpilib.DigitalInput(bottomStop)
self.topStopSwitch = wpilib.DigitalInput(topStop)
self.bendSwitchOne = wpilib.DigitalInput(2)
self.bendSwitchTwo = wpilib.DigitalInput(3)
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 __init__(self, logger):
super().__init__('Elevator')
self._logger = logger
self._motor = wpilib.Talon(robotmap.PWM_Elevator)
self._encoder = wpilib.Encoder(robotmap.DIO_elevatorASource, robotmap.DIO_elevatorBsource,
False, wpilib.Encoder.EncodingType.k1X)
self._encoder.reset()
self._lowerLimitSwitch = wpilib.DigitalInput(robotmap.DIO_elevatorLowerLimit)
self._upperLimitSwitch = wpilib.DigitalInput(robotmap.DIO_elevatorUpperLimit)
def createObjects(self):
# Elevator motor/sensors
self.elevator_motor = wpilib.Talon(1)
self.elevator_top = wpilib.DigitalInput(1)
self.elevator_mid = wpilib.DigitalInput(2)
self.elevator_bottom = wpilib.DigitalInput(3)
self.joy = wpilib.Joystick(0)
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 __init__(self):
Subsystem.__init__(self, "GateShot")
self.gate_shot = WPI_TalonSRX(0)
# Close
self.switchClose = wpilib.DigitalInput(0)
# Open
self.switchOpen = wpilib.DigitalInput(1)
# Switch beneath sushi wheel
self.switchSushi = wpilib.DigitalInput(2)
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, robot):
super().__init__('climber')
self.robot = robot
# Default max value for climber encoders
self.front_encoder_min = -530225
self.front_encoder_max = 0
# set up motor controllers
self.climber_motor_1 = ctre.WPI_TalonSRX(const.CAN_MOTOR_CLIMBER_1)
self.climber_motor_2 = ctre.WPI_VictorSPX(const.CAN_MOTOR_CLIMBER_2)
self.climber_motor_drive = ctre.WPI_TalonSRX(const.CAN_MOTOR_CLIMBER_DRIVE)
self.climber_back_motor = ctre.WPI_VictorSPX(const.CAN_MOTOR_BACK_CLIMBER)
self.climber_motor_1.setInverted(False)
self.climber_motor_2.setInverted(False)
self.climber_motor_drive.setInverted(False)
# PROBABLY NEEDS TO CHANGE
self.climber_back_motor.setInverted(False)
self.climber_kP = 0.02
self.climber_kI = 0.0
self.climber_kD = 0.0
#self.climber_motor_1.configOpenLoopRamp(1)
self.climber_motor_2.follow(self.climber_motor_1)
self.climber_pid = wpilib.PIDController(
self.climber_kP,
self.climber_kI,
self.climber_kD,
self.get_climber_pid_input,
self.set_climber_pid_output,
)
# add methods for range, continuous, tolerance etc.
self.climber_pid.reset()
self.climber_pid.setInputRange(-90, 90)
self.climber_pid.setOutputRange(-1, 1)
self.climber_pid.setContinuous(False)
self.climber_pid.setAbsoluteTolerance(0)
self.stop_climber()
# set up limit switches
self.front_top_limit = wpilib.DigitalInput(const.DIO_CLIMBER_FRONT_TOP_LIMIT)
self.front_bottom_limit = wpilib.DigitalInput(const.DIO_CLIMBER_FRONT_BOTTOM_LIMIT)
self.stilts_hit_platform_limit = wpilib.DigitalInput(const.DIO_CLIMBER_STILTS_HIT_PLATFORM_LIMIT)
self.back_top_limit = wpilib.DigitalInput(const.DIO_CLIMBER_BACK_TOP_LIMIT)
self.back_bottom_limit = wpilib.DigitalInput(const.DIO_CLIMBER_BACK_BOTTOM_LIMIT)
def __init__(self, robot):
# Configure devices
self.rollerMotor = wpilib.Victor(6)
self.ballDetector = wpilib.DigitalInput(10)
self.openDetector = wpilib.DigitalInput(6)
self.piston = wpilib.Solenoid(0, 1)
self.robot = robot
# Put everything to the LiveWindow for testing.
super().__init__("Collector")
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 __init__(self, robot):
print("[ClimbSystem] initializing")
super().__init__("ClimbSystem")
self.robot = robot
left_arm_motor = ctre.WPI_TalonSRX(self.LEFT_ARM_MOTOR_ID)
left_arm_motor.setInverted(False)
left_arm_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
left_arm_motor.setSafetyEnabled(False)
left_arm_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
self.left_arm_motor = left_arm_motor
right_arm_motor = ctre.WPI_TalonSRX(self.RIGHT_ARM_MOTOR_ID)
right_arm_motor.setInverted(True)
right_arm_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
right_arm_motor.setSafetyEnabled(False)
right_arm_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
right_arm_motor.follow(left_arm_motor)
self.right_arm_motor = right_arm_motor
leg_motor = ctre.WPI_TalonSRX(self.CLIMB_LEG_MOTOR_ID)
leg_motor.setInverted(False)
leg_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
leg_motor.setSafetyEnabled(False)
leg_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
self.leg_motor = leg_motor
left_crawl_motor = ctre.WPI_TalonSRX(self.LEFT_CRAWL_MOTOR_ID)
left_crawl_motor.setInverted(False)
left_crawl_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
left_crawl_motor.setSafetyEnabled(False)
left_crawl_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
right_crawl_motor = ctre.WPI_TalonSRX(self.RIGHT_CRAWL_MOTOR_ID)
right_crawl_motor.setInverted(True)
right_crawl_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
right_crawl_motor.setSafetyEnabled(False)
right_crawl_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
self.drive = wpilib.drive.DifferentialDrive(left_crawl_motor,
right_crawl_motor)
self.drive.setSafetyEnabled(False)
self.ultrasonic = wpilib.AnalogInput(self.CLIMB_ULTRASONIC_ID)
self.leg_top_hall_effect = wpilib.DigitalInput(
self.LEG_TOP_HALL_EFFECT_ID)
self.leg_middle_hall_effect = wpilib.DigitalInput(
self.LEG_MIDDLE_HALL_EFFECT_ID)
self.leg_bottom_hall_effect = wpilib.DigitalInput(
self.LEG_BOTTOM_HALL_EFFECT_ID)
def createObjects(self):
"""Initialize all wpilib motors & sensors"""
# setup camera
# setup master and slave drive motors
self.drive_slave_left = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_LEFT_ID)
self.drive_master_left = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_LEFT_ID)
self.drive_master_left.follow(self.drive_slave_left)
self.drive_slave_right = lazytalonfx.LazyTalonFX(
self.DRIVE_SLAVE_RIGHT_ID)
self.drive_master_right = lazytalonfx.LazyTalonFX(
self.DRIVE_MASTER_RIGHT_ID)
self.drive_master_right.follow(self.drive_slave_right)
# setup actuator motors
self.intake_motor = lazytalonsrx.LazyTalonSRX(self.INTAKE_ID)
self.low_tower_motor = lazytalonsrx.LazyTalonSRX(self.LOW_TOWER_ID)
self.high_tower_motor = lazytalonsrx.LazyTalonSRX(self.HIGH_TOWER_ID)
self.turret_motor = lazytalonsrx.LazyTalonSRX(self.TURRET_ID)
self.turret_motor.setEncoderConfig(lazytalonsrx.CTREMag, True)
self.flywheel_motor = rev.CANSparkMax(self.FLYWHEEL_MOTOR_ID,
rev.MotorType.kBrushless)
self.climb_winch_slave = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_SLAVE_ID)
self.climb_winch_master = lazytalonsrx.LazyTalonSRX(
self.CLIMB_WINCH_MASTER_ID)
self.climb_winch_slave.follow(self.climb_winch_master)
self.slider_motor = lazytalonsrx.LazyTalonSRX(self.SLIDER_ID)
# setup imu
self.imu = lazypigeonimu.LazyPigeonIMU(self.intake_motor)
# setup proximity sensors
self.tower_sensors = [wpilib.DigitalInput(i) for i in range(0, 5)]
self.intake_sensors = [wpilib.DigitalInput(8), wpilib.DigitalInput(9)]
# setup joysticks
self.driver = wpilib.Joystick(0)
self.operator = wpilib.XboxController(1)
self.nt = NetworkTables.getTable("robot")
self.manual_indexer = True
self.chassis_slow = False
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.RobotDrive(0,1,2,3)
self.stick = wpilib.Joystick(0)
self.Motor1 = wpilib.VictorSP(4)
self.Motor2 = wpilib.VictorSP(5)
self.Switch1 = wpilib.DigitalInput(0)
self.Switch2 = wpilib.DigitalInput(1)
self.Servo1 = wpilib.Servo(6)
self.Servo2 = wpilib.Servo(7)
def robotInit(self):
'''Robot-wide initialization code should go here'''
# Left-hand stick Y controls left side motor
self.lstick = wpilib.Joystick(0)
# Left-hand stick Z controls right side motor
self.rstick = wpilib.Joystick(1)
self.driveline_subsystem = Driveline(self)
self.limit1 = wpilib.DigitalInput(1)
self.limit2 = wpilib.DigitalInput(2)
self.position = wpilib.AnalogInput(2)
self.autonomous_command = NavigateMaze(self)
def __init__(self, logger):
super().__init__('Arm')
self._logger = logger
self._motor = wpilib.Talon(robotmap.PWM_Arm)
self._encoder = wpilib.Encoder(robotmap.DIO_armASource,
robotmap.DIO_armBsource, False,
wpilib.Encoder.EncodingType.k1X)
self._encoder.reset()
self._lowerLimitSwitch = wpilib.DigitalInput(
robotmap.DIO_armLowerLimit)
self._upperLimitSwitch = wpilib.DigitalInput(
robotmap.DIO_armUpperLimit)
self._inGameLowerLimitSwitch = wpilib.DigitalInput(
robotmap.DIO_armInGameLowerLimit)