def robotInit(self):
bl = wpilib.CANTalon(1)
fl = wpilib.CANTalon(3)
fr = wpilib.CANTalon(0)
br = wpilib.CANTalon(2)
self.talons = [bl, fl, fr, br]
self.pdp = wpilib.PowerDistributionPanel()
def __init__(self, robot):
super().__init__()
self.robot = robot
self.left = wpilib.CANTalon(6)
self.right = wpilib.CANTalon(8)
self.loaded = wpilib.DigitalInput(2)
def robotInit(self):
'''Robot initialization function'''
gyroChannel = 0 #analog input
self.joystickChannel = 0 #usb number in DriverStation
#channels for motors
self.leftMotorChannel = 1
self.rightMotorChannel = 0
self.leftRearMotorChannel = 3
self.rightRearMotorChannel = 2
self.angleSetpoint = 0.0
self.pGain = 1 #propotional turning constant
#gyro calibration constant, may need to be adjusted
#gyro value of 360 is set to correspond to one full revolution
self.voltsPerDegreePerSecond = .0128
self.myRobot = wpilib.RobotDrive(
wpilib.CANTalon(self.leftMotorChannel),
wpilib.CANTalon(self.leftRearMotorChannel),
wpilib.CANTalon(self.rightMotorChannel),
wpilib.CANTalon(self.rightRearMotorChannel))
self.gyro = wpilib.AnalogGyro(gyroChannel)
self.joystick = wpilib.Joystick(self.joystickChannel)
def robotInit(self):
self.gamepad = Gamepad(port=1)
self.sh = wpilib.CANTalon(5)
self.it = wpilib.CANTalon(6)
self.flywheelSpeedLog = LogState("Flywheel speed")
def robotInit(self):
global stick, drive, gyro, highGear
stick = wpilib.Joystick(1)
drive = wpilib.RobotDrive((wpilib.CANTalon(0)), (wpilib.CANTalon(2)),
(wpilib.CANTalon(1)), (wpilib.CANTalon(3)))
drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontLeft, True)
drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearLeft, True)
gyro = wpilib.Gyro(0)
highGear = wpilib.DoubleSolenoid(0, 1)
def robotInit(self):
self.motor = wpilib.CANTalon(1)
self.motor = wpilib.CANTalon(2)
self.motor = wpilib.CANTalon(3)
self.motor = wpilib.CANTalon(4)
self.motor.set(1)
self.robot_drive = wpilib.RobotDrive(0,1)
def module_init(self):
# Setup a device references
self.joystick = wpilib.Joystick(0)
self.left_front_motor = wpilib.CANTalon(1)
self.left_front_motor.enableControl()
self.left_rear_motor = wpilib.CANTalon(2)
self.left_rear_motor.enableControl()
self.right_front_motor = wpilib.CANTalon(3)
self.right_front_motor.enableControl()
self.right_rear_motor = wpilib.CANTalon(4)
self.right_rear_motor.enableControl()
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 robotInit(self):
self.leftRear = wpilib.CANTalon(1)
self.leftRear.enableControl()
self.rightRear = wpilib.CANTalon(3)
self.rightRear.enableControl()
self.leftFront = wpilib.CANTalon(2)
self.leftFront.enableControl()
self.rightFront = wpilib.CANTalon(4)
self.rightFront.enableControl()
self.robotDrive = wpilib.RobotDrive(frontLeftMotor=self.leftFront,
frontRightMotor=self.rightFront,
rearLeftMotor=self.leftRear,
rearRightMotor=self.rightRear)
self.joystick = wpilib.Joystick(0)
def createObjects(self):
#Sticks
self.driverStick = wpilib.Joystick(1)
self.operatorStick = wpilib.Joystick(0)
#CAN Talons
self.lf = wpilib.CANTalon(2)
self.lr = wpilib.CANTalon(3)
self.rf = wpilib.CANTalon(4)
self.rb = wpilib.CANTalon(5)
#robot drive to eventually control our mecanum drive train
self.robotDrive = wpilib.RobotDrive(self.lf, self.lr, self.rf, self.rb)
#Intake motor
self.intakeController = wpilib.CANTalon(6)
#Shooter motor
self.intakeController = wpilib.CANTalon(7)
#Climber motor
self.climbController = wpilib.CANTalon(8)
#agitator
# Maybe be deprecated because of a simpler solution
self.aggitController = wpilib.CANTalon(9)
# IMU Gyroscope
self.navX = navx.AHRS.create_spi()
#network tables
self.sd = NetworkTable.getTable('smartdashboard')
self.viz = NetworkTable.getTable('visiondashboard')
def robotInit(self):
self.gamepad = Gamepad(port=0)
fl = wpilib.CANTalon(2)
fr = wpilib.CANTalon(1)
bl = wpilib.CANTalon(0)
br = wpilib.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self.currentPID = None
self.fastPID = (3.0, 0.0, 3.0, 0.0)
self.fastPIDScale = 0.1
self.slowPID = (30.0, 0.0, 3.0, 0.0)
self.slowPIDScale = 0.01
self.pidLog = LogState("Drive PID")
self._setPID(self.fastPID)
# 4156 ticks per wheel rotation
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 18 teeth and the wheel has 187 teeth
# 187 / 18 * 400 = 4155.5556 = ~4156
# TODO: recalculate ticks per rotation
self.holoDrive = HolonomicDrive(fl, fr, bl, br, 4156)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(22.5)) #angle of rollers
self.drive = AccelerationFilterDrive(self.holoDrive)
self.ahrs = AHRS.create_spi() # the NavX
self.drive = FieldOrientedDrive(self.drive,
self.ahrs,
offset=math.pi / 2)
self.drive.zero()
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.normalScale = 0.3
self.fastScale = 0.5
self.slowScale = 0.05
self.joystickExponent = 2
def __init__(self, ports):
"""
ports is an array of integers for the CANTalon ports to test
"""
wpilib.IterativeRobot.__init__(self)
self.talons = [wpilib.CANTalon(p) for p in ports]
for t in self.talons:
t.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
def __init__(self, robot):
super().__init__(-25, 0, 0)
self.robot = robot
self.tilt_motor = wpilib.CANTalon(10)
self.tilt_pot = wpilib.AnalogInput(0)
self.setAbsoluteTolerance(.01)
def __init__(self, ports):
"""
ports is an array of integers for the CANTalon ports to test
"""
super().__init__()
self.talons = [wpilib.CANTalon(p) for p in ports]
self.logStates = [LogState("Talon " + str(p)) for p in ports]
for t in self.talons:
t.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
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 robotInit(self):
self.secondaryGamepad = seamonsters.gamepad.globalGamepad(port=1)
self.climberMotor = wpilib.CANTalon(4)
self.lockLog = LogState("Climber lock mode")
self.statusLog = LogState("Climber status")
self.currentLog = LogState("Climber current")
#self.encoderLog = LogState("Climber encoder")
self.encoderLog = None
def robotInit(self):
self.servo = wpilib.Servo(6)
##INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
##INITIALIZE MOTORS##
self.lf_motor = wpilib.CANTalon(6)
self.lr_motor = wpilib.CANTalon(2)
self.rf_motor = wpilib.CANTalon(8)
self.rr_motor = wpilib.CANTalon(4)
##ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
def module_init(self):
# Setup a device references
self.joystick = wpilib.Joystick(0)
self.motors = {
"left_drive_0": wpilib.CANTalon(1),
"left_drive_1": wpilib.CANTalon(2),
"right_drive_0": wpilib.CANTalon(4),
"right_drive_1": wpilib.CANTalon(3)
}
self.motors["left_drive_0"].setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
#self.motors["left_drive_0"].configEncoderCodesPerRev(1440)
self.motors["right_drive_0"].setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
#self.motors["right_drive_0"].configEncoderCodesPerRev(1440)
for motor in self.motors:
self.motors[motor].enableBrakeMode(True)
self.navx = navx.AHRS.create_spi()
self.angle_setpoint = 0
self.input_x = 0
self.input_y = 0
self.breach_mode = False
self.speed_mode = False
def __init__(self):
super().__init__()
tFrontLeft = wpilib.CANTalon(0)
tRearLeft = wpilib.CANTalon(1)
tFrontRight = wpilib.CANTalon(2)
tRearRight = wpilib.CANTalon(3)
self.rdRobotDrive = wpilib.RobotDrive(tFrontLeft, tRearLeft,
tFrontRight, tRearRight)
invertDrive = config.isPracticeBot # False for comp bot, True for practice
self.rdRobotDrive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontLeft, invertDrive)
self.rdRobotDrive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, invertDrive)
self.rdRobotDrive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearLeft, invertDrive)
self.rdRobotDrive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, invertDrive)
self.encRightEncoder = wpilib.Encoder(0, 1)
self.encLeftEncoder = wpilib.Encoder(2, 3)
encDPP = 10 * 8.5 / 2 * math.pi / 1440 # E4T has 1440 PPR
# Wheels are 8.5in diameter
self.encRightEncoder.setDistancePerPulse(encDPP)
self.encLeftEncoder.setDistancePerPulse(encDPP)
wpilib.Timer.delay(2000 / 1000)
self.gyro = ITG3200.ITG3200(wpilib.I2C.Port.kOnboard)
wpilib.Timer.delay(50 / 1000)
self.gyro.init()
wpilib.Timer.delay(50 / 1000)
self.gyro.calibrate()
self.gyro.resetAngle()
self.lastThrottle = 0
self.lastTurn = 0
self.startAngle = 0
def robotInit(self):
self.timercounter = 0
'''
self.talon = wpilib.CANTalon(5)
self.talon.changeControlMode(CANTalon.ControlMode.Position)
self.talon.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.talon.setSensorPosition(0)
self.talon.setP(.001)
self.bin_talon = wpilib.CANTalon(15)
self.bin_talon.changeControlMode(CANTalon.ControlMode.Position)
self.bin_talon.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.bin_talon.reverseSensor(True)
self.bin_talon.setSensorPosition(0)
'''
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
self.toteMotor = wpilib.CANTalon(5)
self.canMotor = wpilib.CANTalon(15)
self.reverse = 1
# #SMART DASHBOARD
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(
0.8,
0.0,
1.5, #2.0 * Kp / Tu * 0.1, 1.0 * Kp * Tu / 20.0 * 0,
self.bno055,
self.heading_hold_pid_output)
"""self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)"""
self.heading_hold_pid.setAbsoluteTolerance(3.0 * math.pi / 180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-0.2, 0.2)
#self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
self.joystick_rate = 0.3
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, talonPort, ticksPerRotation=4000, maxVelocity=10):
self.talonPort = talonPort
self.Talon = wpilib.CANTalon(talonPort)
self.Talon.setPID(1, 0, 1, 0)
self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Position)
#self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.goalPosition = self.Talon.getPosition()
self.maxVelocity = maxVelocity
self.ticksPerRotation = ticksPerRotation
self.gamepad = Gamepad(0)
self.adjustFactor = 1.0
self.PIDNumber = 0
def robotInit(self, talonPort, ticksPerRotation=4000, maxVelocity=10):
self.talonPort = talonPort
self.Talon = wpilib.CANTalon(talonPort)
self.Talon.setPID(1, 0, 1, 0)
self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Position)
#self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.goalPosition = self.Talon.getPosition()
self.maxVelocity = maxVelocity
self.ticksPerRotation = ticksPerRotation
self.Joystick = seamonsters.joystick.JoystickUtils(0)
self.JoystickValues = [0, 0, 0, 0]
self.PIDNumber = 0
def __init__(self):
self.flywheelMotor = wpilib.CANTalon(5)
self.speedVoltage = .76
self.speedSpeed = 21000
# encoder resolution is 512 (* 4)
self.flywheelMotor.setPID(0.15, 0.0, 5.0, 0)
self.flywheelMotor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.switchSpeedMode()
self.flywheelMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
def robotInit(self):
self.talon = wpilib.CANTalon(7)
self.joy = wpilib.Joystick(0)
self.loops = 0
# first choose the sensor
self.talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.talon.reverseSensor(False)
#self.talon.configEncoderCodesPerRev(XXX), // if using FeedbackDevice.QuadEncoder
#self.talon.configPotentiometerTurns(XXX), // if using FeedbackDevice.AnalogEncoder or AnalogPot
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputVoltage(+0.0, -0.0)
self.talon.configPeakOutputVoltage(+12.0, 0.0)
# set closed loop gains in slot0
self.talon.setProfile(0)
self.talon.setF(0.1097)
self.talon.setP(0.22)
self.talon.setI(0)
self.talon.setD(0)
def robotInit(self):
# Initialize the CanTalonSRX on device 1.
self.motor = wpilib.CANTalon(1)
# This sets the mode of the m_motor. The options are:
# PercentVbus: basic throttle; no closed-loop.
# Current: Runs the motor with the specified current if possible.
# Speed: Runs a PID control loop to keep the motor going at a constant
# speed using the specified sensor.
# Position: Runs a PID control loop to move the motor to a specified move
# the motor to a specified sensor position.
# Voltage: Runs the m_motor at a constant voltage, if possible.
# Follower: The m_motor will run at the same throttle as the specified
# other talon.
self.motor.changeControlMode(wpilib.CANTalon.ControlMode.Position)
# This command allows you to specify which feedback device to use when doing
# closed-loop control. The options are:
# AnalogPot: Basic analog potentiometer
# QuadEncoder: Quadrature Encoder
# AnalogEncoder: Analog Encoder
# EncRising: Counts the rising edges of the QuadA pin (allows use of a
# non-quadrature encoder)
# EncFalling: Same as EncRising, but counts on falling edges.
self.motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.AnalogPot)
# 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.motor.setPID(1.0, 0.0, 0.0)
def __init__(self, robot):
"""Initialise the drivetrain."""
super().__init__()
#Set up everything
self.robot = robot
#Set the values to 0 to make sure things work without running away
self.twist = 0
self.y = 0
#0-indexed joysticks lol
self.joystick = wpilib.Joystick(0)
#CANTalon motors for the drivetrain.
self.zed = wpilib.CANTalon(9)
self.one = wpilib.CANTalon(2)
self.two = wpilib.CANTalon(3)
self.three = wpilib.CANTalon(7)
self.four = wpilib.CANTalon(5)
self.five = wpilib.CANTalon(4)
#Actual drivetrains. Basically fun.
self.firstSet = wpilib.RobotDrive(self.zed, self.one)
self.secondSet = wpilib.RobotDrive(self.two, self.three)
self.thirdSet = wpilib.RobotDrive(self.four, self.five)
def createObjects(self):
# Joysticks
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# Motors (l/r = left/right, f/r = front/rear)
self.lf_motor = wpilib.CANTalon(5)
self.lr_motor = wpilib.CANTalon(10)
self.rf_motor = wpilib.CANTalon(15)
self.rr_motor = wpilib.CANTalon(20)
# Drivetrain object
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
# Left and right arm motors (there's two, which both control the raising and lowering the arm)
self.leftArm = wpilib.CANTalon(25)
self.rightArm = wpilib.CANTalon(30)
# Motor that spins the bar at the end of the arm.
# There was originally going to be one on the right, but we decided against that in the end.
# In retrospect, that was probably a mistake.
self.leftBall = wpilib.Talon(9)
# Motor that reels in the winch to lift the robot.
self.winchMotor = wpilib.Talon(0)
# Motor that opens the winch.
self.kickMotor = wpilib.Talon(1)
# Aiming flashlight
self.flashlight = wpilib.Relay(0)
# Timer to keep light from staying on for too long
self.lightTimer = wpilib.Timer()
# Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off.
# self.turningOffState keeps track of which on/off it's on.
self.turningOffState = 0
# Is currently on or off? Used to detect if UI button is pressed.
self.lastState = False
# Drive encoders; measure how much the motor has spun
self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True)
self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor)
# Distance sensors
self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0)
self.ultrasonic = wpilib.AnalogInput(1)
# NavX (purple board on top of the RoboRIO)
self.navX = navx.AHRS.create_spi()
# Initialize SmartDashboard, the table of robot values
self.sd = NetworkTable.getTable('SmartDashboard')
# How much will the control loop pause in between (0.025s = 25ms)
self.control_loop_wait_time = 0.025
# Button to reverse controls
self.reverseButton = ButtonDebouncer(self.joystick1, 1)
# Initiate functional buttons on joysticks
self.shoot = ButtonDebouncer(self.joystick2, 1)
self.raiseButton = ButtonDebouncer(self.joystick2, 3)
self.lowerButton = ButtonDebouncer(self.joystick2, 2)
self.lightButton = ButtonDebouncer(self.joystick1, 6)
def __init__(self, robot):
super().__init__()
self.pdp = robot.pdp
self.datalogger = robot.datalogger
self.currentAverageValues = 20
self.armCurrentValues = [0]*self.currentAverageValues
self.angleOffset = config.articulateAngleLow
self.tShooterL = wpilib.CANTalon(4)
self.tShooterR = wpilib.CANTalon(5)
self.tShooterL.setInverted(not config.isPracticeBot) # True for comp bot, false for practice
self.tShooterR.setInverted(config.isPracticeBot) # True for comp bot, false for practice
self.shooterEncoderType = wpilib.CANTalon.FeedbackDevice.CtreMagEncoder_Relative
self.tShooterL.setFeedbackDevice(self.shooterEncoderType)
self.tShooterR.setFeedbackDevice(self.shooterEncoderType)
self.shooterLPID = wpilib.PIDController(Kp=config.shooterLKp, Ki=config.shooterLKi, Kd=0,
kf=config.shooterLKf,
source=lambda: self.tShooterL.getSpeed() *
(-1 if config.isPracticeBot else 1),
output=self.tShooterL.set, period=20/1000) # Fast PID Loop
self.shooterLPID.setOutputRange(-1, 1)
self.shooterLPID.setInputRange(-18700/3, 18700/3)
self.shooterRPID = wpilib.PIDController(Kp=config.shooterRKp, Ki=config.shooterRKi, Kd=0.000,
kf=config.shooterRKf,
source=lambda: self.tShooterR.getSpeed() *
(-1 if not config.isPracticeBot else 1),
output=self.tShooterR.set, period=20/1000) # Fast PID Loop
self.shooterRPID.setOutputRange(-1, 1)
self.shooterRPID.setInputRange(-18700/3, 18700/3)
self.shooterLPID.setSetpoint(0)
self.shooterRPID.setSetpoint(0)
self.shooterLPID.enable()
self.shooterRPID.enable()
self.articulateEncoder = wpilib.Encoder(4, 5)
self.articulateEncoder.reset()
self.vArticulate = wpilib.VictorSP(1)
self.articulateEncoder.setDistancePerPulse(
(1440/(360*4)) * 1.5) # Clicks per degree / Magic numbers
self.articulatePID = wpilib.PIDController(Kp=config.articulateKp, Ki=config.articulateKi, Kd=config.articulateKd,
source=self.getAngle,
output=self.updateArticulate)
self.articulatePID.setOutputRange(-1, +1)
self.articulatePID.setInputRange(config.articulateAngleLow, config.articulateAngleHigh)
self.articulatePID.setSetpoint(35)
self.articulatePID.setPercentTolerance(100/130)
self.articulatePID.enable()
self.vIntake = wpilib.VictorSP(0)
self.sKicker = wpilib.Servo(2)
self.limLow = wpilib.DigitalInput(6)
self.limHigh = wpilib.DigitalInput(7)
self.lastKicker = False
# Data logger
self.datalogger.add_data_source("Left Shooter Motor", self.tShooterL.getSpeed)
self.datalogger.add_data_source("Right Shooter Motor", self.tShooterR.getSpeed)
self.datalogger.add_data_source("Articulate Angle", self.getAngle)
