class GateShot_Subsystem(Subsystem):
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 engageMotor(self, spinSpeed):
self.gate_shot.set(spinSpeed)
print('Gate open')
def disengageMotor(self):
self.gate_shot.set(0)
def readCloseSwitch(self):
return self.switchClose.get()
def readOpenSwitch(self):
return self.switchOpen.get()
def readSushiSwitch(self):
return self.switchSushi.get()
class Drivetrain(Subsystem):
def __init__(self):
# Verify motor ports when placed on frame
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.motor_lf.setInverted(False)
self.motor_lr.setInverted(False)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.timesInMotionMagic = 0
# first choose the sensor
self.talon.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs)
self.talon.setSensorPhase(True)
self.talon.setInverted(False)
# Set relevant frame periods to be at least as fast as periodic rate
self.talon.setStatusFramePeriod(WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs)
self.talon.setStatusFramePeriod(WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs)
# set the peak and nominal outputs
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# set closed loop gains in slot0 - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0.2, self.kTimeoutMs)
self.talon.config_kP(0, 0.2, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.talon.configMotionCruiseVelocity(15000, self.kTimeoutMs)
self.talon.configMotionAcceleration(6000, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def __init__(self, robot):
self.robot = robot
self.leftArm = Talon(self.robot.kCubeGrabber['left_arm'])
self.rightArm = Talon(self.robot.kCubeGrabber['right_arm'])
self.armUS = wpilib.AnalogInput(self.robot.kCubeGrabber['ultra_sonic'])
self.armSwitch = wpilib.DigitalInput(self.robot.kCubeGrabber['switch'])
self.armClosePosition = self.robot.kCubeGrabber['close']
self.armOpenPosition = self.robot.kCubeGrabber['open']
self.driverTwo = self.robot.cStick
self.armSolenoid = wpilib.Solenoid(self.robot.pneumaticControlModuleCANID, self.robot.kCubeGrabber['solenoid'])
"""
Initializes the predetermined distances for grabber/cube interaction.
"""
self.spinDistance = 12
self.closeDistance = 7
"""
Initializes Toggle Counts for the arm functionality.
"""
self.armModeToggleCount = 2
self.openToggleCount = 3
def __init__(self):
# Initialize all controllers
self.driveLeftMaster = Talon(kDriveTrain.lmId)
self.driveLeftSlave = Talon(kDriveTrain.lsId)
self.driveRightMaster = Talon(kDriveTrain.rmId)
self.driveRightSlave = Talon(kDriveTrain.rsId)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
# CURRENTLY DISABLED WHEN USING WITH DifferentialDrive
# self.driveLeftSlave.setInverted(False)
# self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
# Throw data on the SmartDashboard so we can work with it.
SD.putNumber('Left Quad Pos.',
self.driveLeftMaster.getQuadraturePosition())
SD.putNumber('Right Quad Pos.',
self.driveRightMaster.getQuadraturePosition())
# these may give the derivitive an integral of the PID once
# they are set. For now, they just show 0
SD.putNumber('Left Derivative',
self.driveLeftMaster.getErrorDerivative(0))
SD.putNumber('Left Integral',
self.driveLeftMaster.getIntegralAccumulator(0))
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
# self.driveLeftMaster.config_kP(0, .3, 10)
# kP = self.driveLeftMaster.configGetParameter(
# self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)
# SmartDashboard.putNumber('Left Proportional', kP)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
class Mechanisms(Subsystem):
def __init__(self):
# Verify motor ports when placed on frame
self.intake = WPI_TalonSRX(6)
self.intake_solenoid = DoubleSolenoid(2, 3)
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift = DoubleSolenoid(0, 1)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
self.crossbow = WPI_TalonSRX(5)
def set_crossbow(self, speed):
self.crossbow.set(speed)
def get_crossbow(self):
return self.crossbow.get()
def set_intake(self, speed):
self.intake.set(speed)
def pull_intake(self, setting):
self.intake_solenoid.set(setting)
def shift_gears(self, _setting):
self.gear_shift.set(_setting)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def createObjects(self):
""" Create motors, sensors and all your components here. """
self.chassis_left_master = WPI_TalonSRX(1)
self.chassis_left_slave = WPI_TalonSRX(2)
self.chassis_right_master = WPI_TalonSRX(3)
self.chassis_right_slave = WPI_TalonSRX(4)
self.chassis_gyro = AHRS.create_spi()
self.joystick = Joystick(0)
def createObjects(self):
# Inputs
# TODO: Update these dynamically
self.stick = wpilib.Joystick(2)
# self.gamepad = wpilib.XboxController(1)
# self.gamepad_alt = wpilib.XboxController(2)
self.gamepad = wpilib.XboxController(0)
self.gamepad_alt = wpilib.XboxController(1)
# Drive motors
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive_train = wpilib.drive.DifferentialDrive(
self.left_motor, self.right_motor)
self.drive_train.deadband = .04
# Elevator encoder (gearbox)
self.lift_encoder = ExternalEncoder(0, 1)
# TODO: Fix the pid
# self.lift_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Drive encoders
self.left_encoder = ExternalEncoder(2, 3)
self.right_encoder = ExternalEncoder(4, 5, True)
self.left_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
self.right_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION)
# Elevator motors
self.lift_master = CANTalon(2)
self.lift_follower1 = CANTalon(3)
self.lift_follower2 = CANTalon(4)
self.lift_follower1.follow(self.lift_master)
self.lift_follower2.follow(self.lift_master)
# Intake motors
self.left_intake_motor = wpilib.Spark(2)
self.right_intake_motor = wpilib.Spark(3)
self.right_intake_motor.setInverted(True)
# Intake grabbers
self.grabber_solenoid = wpilib.DoubleSolenoid(1, 0, 1)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
wpilib.SmartDashboard.putData("PowerDistributionPanel", self.pdp)
# Misc
self.navx = navx.AHRS.create_spi()
self.net_table = NetworkTables.getTable("SmartDashboard")
self.vision_table = NetworkTables.getTable("limelight")
self.limelight_x = self.vision_table.getEntry("tx")
self.limelight_valid = self.vision_table.getEntry("tv")
self.dashboard_has_target = self.vision_table.getEntry("hastarget")
# Launch camera server
wpilib.CameraServer.launch()
def createObjects(self):
self.chassis_left_motor_master = WPI_TalonSRX(1)
self.chassis_left_motor_slave = WPI_TalonSRX(2)
self.chassis_right_motor_master = WPI_TalonSRX(3)
self.chassis_right_motor_slave = WPI_TalonSRX(4)
self.chassis_navx = AHRS.create_spi()
self.joystick = Joystick(0)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.F = 0.25
SmartDashboard.putNumber("F Gain", self.F)
#self.angle = 0
#SmartDashboard.putNumber("angle", self.angle)
self.range = -1200
self.povPressed = False
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
SmartDashboard.putNumber("Wrist Up Volts", self.wristUpVolts)
SmartDashboard.putNumber("Wrist Down Volts", self.wristDownVolts)
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
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)
class CargoEffector(Subsystem):
def __init__(self):
super().__init__()
self.cargo_motor = WPI_TalonSRX(RM.CARGO_MOTOR)
self.cargo_motor.enableVoltageCompensation(True)
self.cargo_motor.setInverted(True)
def initDefaultCommand(self):
self.setDefaultCommand(self.EatTheBallOrNot(self))
class Stopification(Command):
def __init__(self, cargo_effector):
super().__init__(subsystem=cargo_effector)
self._cargo_effector = cargo_effector
def isFinished(self):
return False
def initialize(self):
self._cargo_effector.cargo_motor.set(0.0)
class EatTheBallOrNot(Command):
def __init__(self, cargo_effector):
super().__init__(subsystem=cargo_effector)
self._cargo_effector = cargo_effector
self._deadband = 0.1
self.logger = logging.getLogger(self.__class__.__name__)
def isFinished(self):
return False
def execute(self):
left_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kLeft)
right_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kRight)
left_trigger = self.apply_deadband(left_trigger)
right_trigger = self.apply_deadband(right_trigger)
# self.logger.info("Left trigger: {}".format(left_trigger))
# self.logger.info("Right trigger: {}".format(right_trigger))
if left_trigger > 0:
self._cargo_effector.cargo_motor.set(left_trigger)
else:
self._cargo_effector.cargo_motor.set(-right_trigger)
def apply_deadband(self, value):
return value if abs(value) > self._deadband else 0.0
class EdibleRegurgitation(enum.IntEnum):
NOMMY = 0
PEWPEW = 1
class Bob(wpilib.IterativeRobot):
def robotInit(self):
# ctre motors defined here
self.r_motor = WPI_TalonSRX(1)
self.l_motor = WPI_TalonSRX(2)
self.stick = wpilib.Joystick(1)
def teleopPeriodic(self):
self.r_motor.set(WPI_TalonSRX.ControlMode.PercentOutput,
self.stick.getY())
class Intake:
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 test(self, right: bool, out: bool):
"""Tests each piston"""
if out:
if right:
self.right_piston.set(self.right_piston.Value.kForward)
else:
self.left_piston.set(self.left_piston.Value.kForward)
else:
if right:
self.right_piston.set(self.right_piston.Value.kReverse)
else:
self.left_piston.set(self.left_piston.Value.kReverse)
def extend(self):
"""Extends pistons"""
self.right_piston_target = self.right_piston.Value.kForward
self.left_piston_target = self.left_piston.Value.kForward
def retract(self):
"""Retracts pistons"""
self.right_piston_target = self.right_piston.Value.kReverse
self.left_piston_target = self.left_piston.Value.kReverse
def speed(self, speed):
"""Sets the intake speed"""
self.collect_speed = speed
def update(self):
"""Updates the values if they are changed"""
if self.right_piston.get() != self.right_piston_target:
self.right_piston.set(self.right_piston_target)
if self.right_piston.get() != self.right_piston_target:
self.right_piston.set(self.right_piston_target)
if self.intake_motor.get() != self.collect_speed:
self.intake_motor.set(self.collect_speed)
def __init__(self):
# Verify motor ports when placed on frame
self.intake = WPI_TalonSRX(6)
self.intake_solenoid = DoubleSolenoid(2, 3)
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift = DoubleSolenoid(0, 1)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
self.crossbow = WPI_TalonSRX(5)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.motor1 = WPI_TalonSRX(1)
self.motor2 = WPI_TalonSRX(2)
self.drive = wpilib.drive.DifferentialDrive(self.motor1, self.motor2)
# joysticks 1 & 2 on the driver station
#self.motor1 = WPI_TalonSRX(2)
self.stick = wpilib.Joystick(0)
def __init__(self):
# Hardware
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def createObjects(self):
# Motors
self.m_lfront = WPI_TalonSRX(1)
self.m_rfront = WPI_TalonSRX(2)
self.m_lback = WPI_TalonSRX(3)
self.m_rback = WPI_TalonSRX(4)
self.m_hatch = wpilib.Spark(0)
self.m_shooter = wpilib.Spark(1)
self.ls_shooter = wpilib.DigitalInput(0)
self.s_intake = wpilib.DoubleSolenoid(2, 3)
# Joysticks (PS4 Controller, in our case)
self.joystick = wpilib.Joystick(0)
class SushiRotator(Subsystem):
def __init__(self):
Subsystem.__init__(self, 'SushiRotator')
# Motor controller object
self.sushi_motor = WPI_TalonSRX(1)
def engageMotor(self, speed):
self.sushi_motor.setSafetyEnabled(False)
self.sushi_motor.set(speed)
def initDefaultCommand(self):
self.setDefaultCommand(Sushi_Act())
class Arm():
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_TalonSRX(10)
self.speed = 1
def update(self):
power = 0
if (self.operator.getArmUp()):
power += self.speed
if (self.operator.getArmDown()):
power -= self.speed
self.motor.set(ControlMode.PercentOutput, power)
def initialize(self):
timeout = 15
self.wristPowerSet = 0
SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet)
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
self.xbox = oi.getJoystick(2)
self.out = 0
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
self.bottomSwitch = DI(map.bottomSwitch)
self.topSwitch = DI(map.topSwitch)
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 robotInit(self):
# self.gyro = wpilib.I2C(wpilib.I2C.Port.kOnboard, 0x68)
self.leftFront = WPI_TalonSRX(5)
self.leftBack = WPI_TalonSRX(6)
self.rightFront = WPI_TalonSRX(7)
self.rightBack = WPI_TalonSRX(8)
self.leftDrive = wpilib.SpeedControllerGroup(self.leftBack,
self.leftFront)
self.rightDrive = wpilib.SpeedControllerGroup(self.rightBack,
self.rightFront)
self.drive = wpilib.drive.DifferentialDrive(self.leftDrive,
self.rightDrive)
self.joystick = wpilib.Joystick(0)
def teleopInit(self):
"""Called when teleop starts; optional"""
self.intake.intake_clamp(False)
self.intake.intake_push(False)
self.lift_motor = WPI_TalonSRX(0)
self.motion.enabled = False
self.chassis.set_inputs(0, 0, 0)
self.intake.extension(True)
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.lastButton1 = False
self.targetPos = 0
# choose the sensor and sensor direction
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# choose to ensure sensor is positive when output is positive
self.talon.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.talon.setInverted(False)
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# Set the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0, self.kTimeoutMs)
self.talon.config_kP(0, 0.1, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx,
self.kTimeoutMs)
class DriveTrain(Subsystem):
'''
This example subsystem controls a single Talon in PercentVBus mode.
'''
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)
# self.motor = wpilib.Talon(1)
def driveCartesian(self, ySpeed, xSpeed, zRotation, gyroAngle=0.0):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def initDefaultCommand(self):
self.setDefaultCommand(FollowJoystick())
def set_crossbow(self, setting):
self.crossbow.set(setting)
def set(self, ySpeed, xSpeed, zRotation, gyroAngle):
self.drive.driveCartesian(ySpeed, xSpeed, zRotation, gyroAngle)
def createObjects(self):
if subsystems_enabled['limelight']:
self.limelight_table = NetworkTables.getTable("limelight")
if subsystems_enabled['navx']:
self.navx = navx.AHRS.create_spi()
if subsystems_enabled['shooter']:
self.shooter_srx_a = WPI_TalonSRX(1)
self.shooter_srx_b = WPI_TalonSRX(2)
if subsystems_enabled['neo']:
self.neo_motor = CANSparkMax(3, MotorType.kBrushless)
if subsystems_enabled['camera']:
wpilib.CameraServer.launch()
self.driver = DriverController(wpilib.XboxController(0))
self.sysop = SysopController(wpilib.XboxController(1))
def robotInit(self):
"""Create motors and stuff here"""
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
# Left side
self.drive_motor_a = WPI_TalonSRX(0)
self.drive_motor_b = WPI_TalonSRX(1)
self.left = wpilib.SpeedControllerGroup(self.drive_motor_a, self.drive_motor_b)
self.left.setInverted(True)
# Right side
self.drive_motor_c = WPI_TalonSRX(2)
self.drive_motor_d = WPI_TalonSRX(3)
self.right = wpilib.SpeedControllerGroup(self.drive_motor_c, self.drive_motor_d)
self.right.setInverted(True)
self.drive_base = wpilib.drive.DifferentialDrive(self.left, self.right)
class Lift(Subsystem):
def __init__(self):
super().__init__("Lift")
self.talon_lift = WPI_TalonSRX(robotmap.talon_lift)
@classmethod
def setSpd(cls, spd_new):
robotmap.spd_lift = spd_new
def raiseLift(self):
self.talon_lift.setInverted(False)
self.talon_lift.set(robotmap.spd_lift)
def lowerLift(self):
self.talon_lift.setInverted(True)
self.talon_lift.set(robotmap.spd_lift_lower)
def stop(self):
self.talon_lift.stopMotor()
