def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
#self.lf_motor = wpilib.Victor(0b00) # => 0
#self.lr_motor = wpilib.Victor(0b01) # => 1
#self.rf_motor = wpilib.Victor(0b10) # => 2
#self.rr_motor = wpilib.Victor(0b11) # => 3
# TODO: This is not in any way an ideal numbering system.
# The PWM ports should be redone to use the old layout above.
self.lf_motor = wpilib.Victor(9)
self.lr_motor = wpilib.Victor(8)
self.rf_motor = wpilib.Victor(7)
self.rr_motor = wpilib.Victor(6)
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(
self.intake_wheel_left, self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
#self.myRobot = wpilib.RobotDrive(0, 1)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
# joyStick 0
self.joyStick = wpilib.Joystick(0)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
# encoders
self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(2,3, False, wpilib.Encoder.EncodingType.k4X)
# set up encoder
self.drivePulsePerRotation = 1024
self.driveWheelRadius = 3.0
self. driveGearRatio = 1.0/1.0
self.driveEncoderPulsePerRot = self.drivePulsePerRotation * self.driveGearRatio
self.driveEncoderDistPerPulse = (math.pi*2*self.driveWheelRadius)/self.driveEncoderPulsePerRot;
self.leftEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.leftEncoder.setReverseDirection(False)
self.rightEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.rightEncoder.setReverseDirection(False)
self.timer = wpilib.Timer()
def __init__(self, port1, port2, port3, port4):
"""Depends on four motors, will make two-motor version asap"""
self.FL = wpi.Victor(port1)
self.FR = wpi.Victor(port2)
self.RL = wpi.Victor(port3)
self.RR = wpi.Victor(port4)
self.vicList = [self.FL, self.FR, self.RL, self.RR]
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()
def createObjects(self):
"""
Initialize testbench components.
"""
self.joystick = wpilib.Joystick(0)
self.brushless = wpilib.NidecBrushless(9, 9)
self.spark = wpilib.Spark(4)
self.lf_victor = wpilib.Victor(0)
self.lr_victor = wpilib.Victor(1)
self.rf_victor = wpilib.Victor(2)
self.rr_victor = wpilib.Victor(3)
self.lf_talon = WPI_TalonSRX(5)
self.lr_talon = WPI_TalonSRX(10)
self.rf_talon = WPI_TalonSRX(15)
self.rr_talon = WPI_TalonSRX(20)
self.drive = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_victor, self.lr_victor,
self.lf_talon, self.lr_talon),
wpilib.SpeedControllerGroup(self.rf_victor, self.rr_victor,
self.rf_talon, self.rr_talon))
wpilib.CameraServer.launch()
def __init__(self, robot):
self.robot = robot
# Configure drive motors
self.frontLeftCIM = wpilib.Victor(1)
self.frontRightCIM = wpilib.Victor(2)
self.backLeftCIM = wpilib.Victor(3)
self.backRightCIM = wpilib.Victor(4)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Left CIM", self.frontLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Front Right CIM", self.frontRightCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Left CIM", self.backLeftCIM)
wpilib.LiveWindow.addActuator("DriveTrain", "Back Right CIM", self.backRightCIM)
# Configure the RobotDrive to reflect the fact that all our motors are
# wired backwards and our drivers sensitivity preferences.
self.drive = wpilib.RobotDrive(self.frontLeftCIM, self.frontRightCIM, self.backLeftCIM, self.backRightCIM)
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(.1)
self.drive.setSensitivity(.5)
self.drive.setMaxOutput(1.0)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft, True)
self.drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# Configure encoders
self.rightEncoder = wpilib.Encoder(1, 2,
reverseDirection=True,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(3, 4,
reverseDirection=False,
encodingType=wpilib.Encoder.EncodingType.k4X)
self.rightEncoder.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
self.leftEncoder.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement)
if robot.isReal():
# Converts to feet
self.rightEncoder.setDistancePerPulse(0.0785398)
self.leftEncoder.setDistancePerPulse(0.0785398)
else:
# Convert to feet 4in diameter wheels with 360 tick simulated encoders.
self.rightEncoder.setDistancePerPulse((4*math.pi)/(360*12))
self.leftEncoder.setDistancePerPulse((4*math.pi)/(360*12))
wpilib.LiveWindow.addSensor("DriveTrain", "Right Encoder", self.rightEncoder)
wpilib.LiveWindow.addSensor("DriveTrain", "Left Encoder", self.leftEncoder)
# Configure gyro
# -> the original pacgoat example is at channel 2, but that was before WPILib
# moved to zero-based indexing. You need to change the gyro channel in
# /usr/share/frcsim/models/PacGoat/robots/PacGoat.SDF, from 2 to 0.
self.gyro = wpilib.AnalogGyro(0)
if robot.isReal():
# TODO: Handle more gracefully
self.gyro.setSensitivity(0.007)
wpilib.LiveWindow.addSensor("DriveTrain", "Gyro", self.gyro)
super().__init__()
def __init__(self, robot):
#Motor Setup
self.frontRightMotor = wpilib.Victor(2) #Yellow doesn't
self.frontLeftMotor = wpilib.Victor(3) #Blue
self.backRightMotor = wpilib.Victor(0) #Red
self.backLeftMotor = wpilib.Victor(5) #Orange doesn't
#Mechanum Drive setup
robot.drive = MecanumDrive(self.frontLeftMotor, self.backLeftMotor,
self.frontRightMotor, self.backRightMotor)
def __init__(self, robot):
# Wheel Setup
self.spinnyWheel = wpilib.Victor(4)
# Flicker Setup
self.flicker = wpilib.Victor(6)
# Misc Variables Setup
self.wheelSpeed = -.5
self.flickerSpeed = .3
self.reverseFlickerSpeed = -.1
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontRightMotor = wpilib.Victor(0)
self.rearRightMotor = wpilib.Victor(1)
self.frontLeftMotor = wpilib.Victor(2)
self.rearLeftMotor = wpilib.Victor(3)
# object that handles basic intake operations
self.omnom_left_motor = wpilib.Spark(7) # make sure channels are correct
self.omnom_right_motor = wpilib.Spark(8)
# object that handles basic lift operations
self.liftMotor = wpilib.Spark(4) # make sure channel is correct
# object that handles basic climb operations
self.winch1 = wpilib.Spark(5)
self.winch2 = wpilib.Spark(6)
# defining motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
# setting up drive group for drive motors
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# defining omnom motor groups
self.omnom_left = wpilib.SpeedControllerGroup(self.omnom_left_motor)
self.omnom_right = wpilib.SpeedControllerGroup(self.omnom_right_motor)
# setting up omnom group for omnom motors
self.omnom = DifferentialDrive(self.omnom_left, self.omnom_right)
self.omnom.setExpiration(0.1)
# defines timer for autonomous
self.timer = wpilib.Timer()
# joystick 0 & on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.stick = wpilib.Joystick(2)
# initialization of the hall-effect sensors
self.DigitalInput = wpilib.DigitalInput(1)
# initialization of the FMS
self.DS = DriverStation.getInstance()
self.PS = DriverStation.getInstance()
# initialization of the camera server
wpilib.CameraServer.launch()
def robotInit(self):
'''Robot initialization function'''
self.stick = wpilib.Joystick(0)
self.rstick = wpilib.Joystick(1)
self.motor_left = wpilib.Victor(0)
self.motor_right = wpilib.Victor(1)
self.motor_gobbler = wpilib.Victor(2)
self.motor_shooter = wpilib.Victor(3)
self.speed_gobbler = 0
self.speed_shooter = 0
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.myRobot = wpilib.RobotDrive(wpilib.Victor(0), wpilib.Victor(1))
self.myRobot.setExpiration(0.1)
self.launcherTop = wpilib.Victor(2)
self.launcherBottom = wpilib.Victor(3)
self.ballIntake = wpilib.Victor(4)
# joysticks 1 & 2 on the driver station
self.stick = wpilib.Joystick(0)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
#self.myRobot = wpilib.RobotDrive(0, 1)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
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 initilization function '''
# initialize networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('shooter_speed', self.shooter_speed)
self.smart_dashboard.putNumber('gear_arm_opening_speed',
self.gear_arm_opening_speed)
self.smart_dashboard.putNumber('gear_arm_closing_speed',
self.gear_arm_closing_speed)
self.smart_dashboard.putNumber('loader_speed', self.loader_speed)
# initialize and launch the camera
wpilib.CameraServer.launch()
# object that handles basic drive operatives
self.drive_rf_motor = wpilib.Victor(portmap.motors.right_front)
self.drive_rr_motor = wpilib.Victor(portmap.motors.right_rear)
self.drive_lf_motor = wpilib.Victor(portmap.motors.left_front)
self.drive_lr_motor = wpilib.Victor(portmap.motors.left_rear)
self.shooter_motor = wpilib.Victor(portmap.motors.shooter)
self.gear_arm_motor = wpilib.Spark(portmap.motors.gear_arm)
self.loader_motor = wpilib.Spark(portmap.motors.loader)
# initialize drive
self.drive = wpilib.RobotDrive(self.drive_lf_motor,
self.drive_lr_motor,
self.drive_rf_motor,
self.drive_rr_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(1)
# initialize autonomous components
self.components = {
'drive': self.drive,
'gear_arm_motor': self.gear_arm_motor
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def createObjects(self):
self.leftStick = wpilib.Joystick(2)
self.elevatorStick = wpilib.Joystick(1)
self.rightStick = wpilib.Joystick(0)
self.elevatorMotorOne = wpilib.Victor(2)
self.elevatorMotorTwo = wpilib.Victor(3)
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.myRobot = DifferentialDrive(self.left, self.right)
self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne,
self.elevatorMotorTwo)
self.elevatorPot = wpilib.AnalogPotentiometer(0)
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.gearshift.set(1)
def __init__(self):
super().__init__()
self.motor = wpilib.Victor(7)
self.contact = wpilib.DigitalInput(5)
# Let's show everything on the LiveWindow
wpilib.LiveWindow.addActuator("Claw", "Motor", self.motor)
wpilib.LiveWindow.addActuator("Claw", "Limit Switch", self.contact)
def __init__(self, port, jaguar, slot=4):
print("speed_con obj on port " + str(port) + " active")
if jaguar == True:
self.vic = wpi.Jaguar(port)
print("is jaguar\n")
else:
self.vic = wpi.Victor(port)
print("is victor\n")
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # => 0
self.lr_motor = wpilib.Victor(0b01) # => 1
self.rf_motor = wpilib.Victor(0b10) # => 2
self.rr_motor = wpilib.Victor(0b11) # => 3
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
def robotInit(self):
""" Robot Initialization. Runs on turn-on. """
### Xnabled? ###
self.true_run = wpilib.SendableChooser()
self.true_run.addDefault("On", True)
self.true_run.addObject("Off", False)
### Setup Camera ###
wpilib.CameraServer.launch("vision.py:main")
### Drive Train Initialization ###
# Motors
self.pwm = [None, None, None, None, None, None, None, None, None, None]
self.pwm[0] = wpilib.Victor(0)
self.pwm[1] = wpilib.Victor(1)
self.pwm[2] = wpilib.Spark(2)
self.pwm[3] = wpilib.Spark(3)
self.pwm[7] = wpilib.Victor(7)
self.pwm[8] = wpilib.Victor(8)
self.pwm[9] = wpilib.Victor(9)
# Drive Train
self.driveLeft = wpilib.SpeedControllerGroup(self.pwm[0], self.pwm[2])
self.driveRight = wpilib.SpeedControllerGroup(self.pwm[1], self.pwm[3])
self.driveTrain = wpilib.drive.DifferentialDrive(
self.driveLeft, self.driveRight)
### Initialize Gamepad ###
self.gamepad = wpilib.XboxController(0)
### Other Inputs ###
self.finderA = wpilib.AnalogInput(0)
self.finderB = wpilib.AnalogInput(1)
### Variables ###
self.launchTick = 0
self.allignment = True
self.direction = -1
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 robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.driveRight = wpilib.Victor(0)
self.driveLeft = wpilib.Victor(1)
self.myRobot = wpilib.RobotDrive(self.driveRight, self.driveLeft)
self.myRobot.setExpiration(0.1)
self.launcherTop = wpilib.Spark(2)
self.launcherBottom = wpilib.Spark(3)
self.ballIntake = wpilib.Victor(4)
self.winch1 = wpilib.Victor(5)
self.winch2 = wpilib.Victor(6)
self.armIn = wpilib.Solenoid(0)
self.armOut = wpilib.Solenoid(1)
self.liftUp = wpilib.Solenoid(2)
self.liftDown = wpilib.Solenoid(3)
# joysticks 1 & 2 on the driver station
self.stick = wpilib.Joystick(0)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontRightMotor = wpilib.Victor(0)
self.rearRightMotor = wpilib.Victor(1)
self.frontLeftMotor = wpilib.Victor(2)
self.rearLeftMotor = wpilib.Victor(3)
# object that handles basic intake operations
self.omnom = wpilib.Spark(7) # make sure channels are correct
# object that handles basic climb operations
self.liftMotor = wpilib.Spark(4) # make sure channel is correct
# defining motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
# setting up drive group for drive motors
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# defines timer for autonomous
self.timer = wpilib.Timer()
# joystick 1 & 2 on the driver station
self.stick = wpilib.Joystick(0)
self.stick2 = wpilib.Joystick(1)
# initialization of the camera server
wpilib.CameraServer.launch()
# initialization of the ultrasonic sensors
self.AnalogInput_one = wpilib.AnalogInput(2)
self.AnalogInput_two = wpilib.AnalogInput(3)
# initialization of the hall-effect sensors
self.DigitalInput = wpilib.DigitalInput(2)
def robotInit(self):
frontLeft = wpilib.Victor(0)
frontRight = wpilib.Victor(1)
rearLeft = wpilib.Victor(2)
rearRight = wpilib.Victor(3)
self.robot_drive = wpilib.RobotDrive(frontLeftMotor=frontLeft,
frontRightMotor=frontRight,
rearLeftMotor=rearLeft,
rearRightMotor=rearRight)
self.joystick = wpilib.Joystick(0)
self.gyro = wpilib.Gyro(0)
#calibrate the gyro
self.gyro_drift = 0.0
wpilib.Timer.delay(2.0)
last_angle = self.gyro.getAngle()
wpilib.Timer.delay(10.0)
self.gyro_drift = (self.gyro.getAngle() - last_angle) / 10.0
self.timer = wpilib.Timer()
self.dashboard = dashboard.Dashboard()
def send_thread():
while True:
self.dashboard.send_udp(dashboard.encode_gyro(
self.get_angle()))
wpilib.Timer.delay(0.05)
def recv_thread():
while True:
self.dashboard.get_msg()
t_send = threading.Thread(target=send_thread)
t_send.daemon = True
t_send.start()
t_recv = threading.Thread(target=recv_thread)
t_recv.daemon = True
def robotInit(self):
Command.getRobot = lambda _: self
wpilib.CameraServer.launch('vision.py:main')
NetworkTables.initialize(server='10.56.54.2')
self.rf_motor = ctre.WPI_VictorSPX(1)
self.rr_motor = ctre.WPI_VictorSPX(2)
self.lf_motor = ctre.WPI_VictorSPX(5)
self.lr_motor = ctre.WPI_VictorSPX(4)
self.drive = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.drivetrain = Drivetrain(self.drive)
self.shooter_motor = ctre.WPI_VictorSPX(3)
self.shooter = Shooter(self.shooter_motor)
self.intake_belt_motor = wpilib.Victor(0)
self.intake_belt = ConveyorBelt(self.intake_belt_motor)
self.magazine_belt_motor = wpilib.Victor(1)
self.magazine_belt = ConveyorBelt(self.magazine_belt_motor,
negate=True)
self.shooter_belt_motor = wpilib.Victor(2)
self.shooter_belt = ConveyorBelt(self.shooter_belt_motor, negate=True)
self.conveyor_mode = 0
self.joystick = oi.get_joystick()
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
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 __init__(self, robot):
super().__init__(self.kP_real, 0, 0)
# Check for simulation and update PID values
# if robot.isSimulation():
# self.getPIDController().setPID(self.kP_simulation, 0, 0, 0)
self.setAbsoluteTolerance(2.5)
self.motor = wpilib.Victor(6)
# Conversion value of potentiometer varies between the real world and simulation
if robot.isReal():
self.pot = wpilib.AnalogPotentiometer(3, -270 / 5)
else:
self.pot = wpilib.AnalogPotentiometer(3) # defaults to degrees
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.
wpilib.LiveWindow.addActuator("Collector", "Roller Motor",
self.rollerMotor)
wpilib.LiveWindow.addSensor("Collector", "Ball Detector",
self.ballDetector)
wpilib.LiveWindow.addSensor("Collector", "Claw Open Detector",
self.openDetector)
wpilib.LiveWindow.addActuator("Collector", "Piston", self.piston)
super().__init__()
def robotInit(self):
#cam.main()
self.robot_drive = wpilib.RobotDrive(wpilib.Spark(self.lf_motor),
wpilib.Spark(self.rf_motor),
wpilib.Spark(self.lr_motor),
wpilib.Spark(self.rr_motor))
self.robot_drive.setExpiration(0.1)
self.robot_drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft,
True)
self.robot_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, True)
self.js = wpilib.Joystick(self.js_channel)
# Mechs
self.climer = wpilib.Spark(self.climer_motor)
self.intake = wpilib.Spark(self.intake_motor)
self.shootor = ctre.cantalon.CANTalon(self.shooter_motor,
controlPeriodMs=10,
enablePeriodMs=50)
self.ha = wpilib.Victor(self.ha_motor)
self.switch1 = wpilib.DigitalInput(self.but1channel)
self.switch2 = wpilib.DigitalInput(self.but1channel2)
#Relay
#self.rha = wpilib.Relay(self.rha_channel, direction=None)
#Switch
#self.switch1.free()
self.shootor.changeControlMode(ctre.CANTalon.ControlMode.Position)
self.shootor.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
# This sets the basic P, I , and D values (F, Izone, and rampRate can also
# be set, but are ignored here).
# These must all be positive floating point numbers (reverseSensor will
# multiply the sensor values by negative one in case your sensor is flipped
# relative to your motor).
# These values are in units of throttle / sensor_units where throttle ranges
# from -1023 to +1023 and sensor units are from 0 - 1023 for analog
# potentiometers, encoder ticks for encoders, and position / 10ms for
# speeds.
self.shootor.setPID(1.0, 0.0, 0.0)
