class DriveTrain(Subsystem):
"""
The DriveTrain subsytem is used by the driver as well as the Pathfinder and Motion Profile
controllers. The default command is DriveJoystick. Each side of the differential drive is
connected to CTRE's magnetic encoders.
"""
ENCODER_TICKS_PER_REV = 4096
MP_SLOT0_SELECT = 0
MP_SLOT1_SELECT = 0
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)
# Set the front motors to be the followers of the rear motors
self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_RIGHT_MOTOR)
# Add the motors to the speed controller groups and create the differential drivetrain
self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
self.rightDrive = SpeedControllerGroup(self.frontRight,
self.rightTalon)
self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)
# Setup the default motor controller setup
self.initControllerSetup()
# Map the pigeon. This will be connected to an unused Talon.
self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
self.pigeonIMU = PigeonIMU(self.talonPigeon)
def initControllerSetup(self):
"""
This method will setup the default settings of the motor controllers.
"""
# Feedback sensor phase
self.leftTalon.setSensorPhase(True)
self.rightTalon.setSensorPhase(True)
# Diable the motor-safety
self.diffDrive.setSafetyEnabled(False)
# Enable brake/coast mode
self.leftTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)
self.rightTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)
# This function will intiliaze the drivetrain motor controllers to the factory defaults.
# Only values which do not match the factory default will be written. Any values which
# are explicity listed will be skipped (ie any values written prior in this method).
# ***** TODO ***** #
def initiaizeDrivetrainMotionProfileControllers(self, stream_rate):
"""
This method will initialize the Talon's for motion profiling
"""
# Invert right motors
self.rightTalon.setInverted(True)
self.frontRight.setInverted(True)
# Enable voltage compensation for 12V
self.leftTalon.configVoltageCompSaturation(12.0, 10)
self.leftTalon.enableVoltageCompensation(True)
self.rightTalon.configVoltageCompSaturation(12.0, 10)
self.rightTalon.enableVoltageCompensation(True)
# PIDF slot index 0 is for autonomous wheel postion
# There are 4096 encoder units per rev. 1 rev of the wheel is pi * diameter. That
# evaluates to 2607.6 encoder units per foot. For the feed-forward system, we expect very
# tight position control, so use a P-gain which drives full throttle at 8" of error. This
# evaluates to 0.588 = (1.0 * 1023) / (8 / 12 * 2607.6)
self.leftTalon.config_kP(0, 0.0, 10)
self.leftTalon.config_kI(0, 0.0, 10)
self.leftTalon.config_kD(0, 0.0, 10)
self.leftTalon.config_kF(0, 1023 / 12, 10) # 10-bit ADC / 12 V
self.leftTalon.config_IntegralZone(0, 100, 10)
self.leftTalon.configClosedLoopPeakOutput(0, 1.0, 10)
self.rightTalon.config_kP(0, 0.0, 10)
self.rightTalon.config_kI(0, 0.0, 10)
self.rightTalon.config_kD(0, 0.0, 10)
self.rightTalon.config_kF(0, 1023 / 12, 10) # 10-bit ADC / 12 V
self.rightTalon.config_IntegralZone(0, 100, 10)
self.rightTalon.configClosedLoopPeakOutput(0, 1.0, 10)
# PIDF slot index 1 is for autonomous heading
self.leftTalon.config_kP(1, 0, 10)
self.leftTalon.config_kI(1, 0, 10)
self.leftTalon.config_kD(1, 0, 10)
self.leftTalon.config_kF(1, 0, 10)
self.leftTalon.config_IntegralZone(1, 100, 10)
self.leftTalon.configClosedLoopPeakOutput(1, 1.0, 10)
self.rightTalon.config_kP(1, 0, 10)
self.rightTalon.config_kI(1, 0, 10)
self.rightTalon.config_kD(1, 0, 10)
self.rightTalon.config_kF(1, 0, 10)
self.rightTalon.config_IntegralZone(1, 100, 10)
self.rightTalon.configClosedLoopPeakOutput(1, 1.0, 10)
# Change the control frame period
self.leftTalon.changeMotionControlFramePeriod(stream_rate)
self.rightTalon.changeMotionControlFramePeriod(stream_rate)
# Initilaize the quadrature encoders and pigeon IMU
self.initQuadratureEncoder()
self.initPigeonIMU()
def cleanUpDrivetrainMotionProfileControllers(self):
'''
This mothod will be called to cleanup the Talon's motion profiling
'''
# Invert right motors again so the open-loop joystick driving works
self.rightTalon.setInverted(False)
self.frontRight.setInverted(False)
# Change the control frame period back to the default
framePeriod = TALON_DEFAULT_MOTION_CONTROL_FRAME_PERIOD_MS
self.leftTalon.changeMotionControlFramePeriod(framePeriod)
self.rightTalon.changeMotionControlFramePeriod(framePeriod)
def initPigeonIMU(self):
# false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
# true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
self.rightTalon.configAuxPIDPolarity(False, 10)
self.leftTalon.configAuxPIDPolarity(True, 10)
# select a gadgeteer pigeon for remote 0
self.rightTalon.configRemoteFeedbackFilter(
self.talonPigeon.getDeviceID(),
RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)
self.leftTalon.configRemoteFeedbackFilter(
self.talonPigeon.getDeviceID(),
RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)
# Select the remote feedback sensor for PID1
self.rightTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)
self.leftTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)
# Using the config feature, scale units to 3600 per rotation. This is nice as it keeps
# 0.1 deg resolution, and is fairly intuitive.
self.rightTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)
self.leftTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)
# Zero the sensor
self.pigeonIMU.setYaw(0, 10)
self.pigeonIMU.setAccumZAngle(0, 10)
def initQuadratureEncoder(self):
"""
This method will initialize the encoders for quadrature feedback.
"""
self.leftTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
self.rightTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
self.leftTalon.getSensorCollection().setQuadraturePosition(0, 10)
self.rightTalon.getSensorCollection().setQuadraturePosition(0, 10)
def getLeftQuadraturePosition(self):
"""
This method will return the left-side sensor quadrature position. The sign needs to
manually be handled here since this function is used to provide the sensor postion outide
of the talon.
"""
return -self.leftTalon.getSensorCollection().getQuadraturePosition()
def getRightQuadraturePosition(self):
"""
This method will return the right-side sensor quadrature position. The sign needs to
manually be handled here since this function is used to provide the sensor postion outide
of the talon.
"""
return self.rightTalon.getSensorCollection().getQuadraturePosition()
def setQuadratureStatusFramePeriod(self, sample_period_ms):
"""
This method will set the status frame persiod of the quadrature encoder
"""
self.leftTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
sample_period_ms, 10)
self.rightTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
sample_period_ms, 10)
def setDefaultQuadratureStatusFramePeriod(self):
"""
This method will set the status frame persiod of the quadrature encoder back to the factory
default.
"""
self.leftTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)
self.rightTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)
def pathFinderDrive(self, leftOutput, rightOutput):
"""
This method will take the Pathfinder Controller motor output and apply them to the
drivetrain.
"""
self.leftTalon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftOutput)
self.rightTalon.set(WPI_TalonSRX.ControlMode.PercentOutput,
-rightOutput)
def getLeftVelocity(self):
return self.leftTalon.getSensorCollection().getQuadratureVelocity()
def getRightVelocity(self):
return self.rightTalon.getSensorCollection().getQuadratureVelocity()
def getLeftVoltage(self):
return self.leftTalon.getMotorOutputVoltage()
def getRightVoltage(self):
return self.rightTalon.getMotorOutputVoltage()
def initDefaultCommand(self):
"""
This method will set the default command for this subsystem.
"""
self.setDefaultCommand(DriveJoystick(self.robot))
class Robot(MagicRobot):
drivetrain = Drivetrain
climber = Climber
elevator = Elevator
intake = Intake
mode = RobotMode.switch
rumbling = False
def createObjects(self):
wpilib.CameraServer.launch()
wpilib.LiveWindow.disableAllTelemetry()
self.left_drive_motor = WPI_TalonSRX(0)
WPI_TalonSRX(1).set(WPI_TalonSRX.ControlMode.Follower,
self.left_drive_motor.getDeviceID())
self.right_drive_motor = WPI_TalonSRX(2)
WPI_TalonSRX(3).set(WPI_TalonSRX.ControlMode.Follower,
self.right_drive_motor.getDeviceID())
self.robot_drive = wpilib.drive.DifferentialDrive(
self.left_drive_motor, self.right_drive_motor)
self.r_intake_motor = WPI_VictorSPX(4)
self.l_intake_motor = WPI_VictorSPX(5)
self.elevator_winch = WPI_TalonSRX(6)
self.climber_motor = WPI_TalonSRX(7)
self.wrist_motor = WPI_TalonSRX(8)
self.intake_ir = wpilib.AnalogInput(0)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 3)
self.right_drive_joystick = wpilib.Joystick(0)
self.left_drive_joystick = wpilib.Joystick(1)
self.operator_joystick = wpilib.Joystick(2)
self.compressor = wpilib.Compressor()
self.elevator_limit_switch = wpilib.DigitalInput(0)
self.climber_motor = WPI_TalonSRX(7)
self.navx = AHRS.create_spi()
self.path_tracking_table = NetworkTables.getTable("path_tracking")
self.down_button = ButtonDebouncer(self.operator_joystick, 1)
self.right_button = ButtonDebouncer(self.operator_joystick, 2)
self.left_button = ButtonDebouncer(self.operator_joystick, 3)
self.has_cube_button = ButtonDebouncer(self.operator_joystick, 5)
self.up_button = ButtonDebouncer(self.operator_joystick, 4)
self.left_bumper_button = JoystickButton(self.operator_joystick, 5)
self.right_bumper_button = JoystickButton(self.operator_joystick, 6)
def up_mode(self):
self.mode += 1
def down_mode(self):
self.mode -= 1
def autonomous(self):
self.intake.reset_wrist()
super().autonomous()
def teleopInit(self):
self.intake.reset_wrist_up()
def teleopPeriodic(self):
self.right = -self.right_drive_joystick.getRawAxis(1)
self.left = -self.left_drive_joystick.getRawAxis(1)
self.right = 0 if abs(self.right) < 0.1 else self.right
self.left = 0 if abs(self.left) < 0.1 else self.left
self.drivetrain.tank(math.copysign(self.right**2, self.right),
math.copysign(self.left**2, self.left))
# outtake
if self.operator_joystick.getRawAxis(3) > 0.01:
self.intake.set_speed(
self.operator_joystick.getRawAxis(3) * 0.8 + 0.2)
elif self.operator_joystick.getRawButton(3):
self.intake.intake()
else:
self.intake.hold()
elevator_speed = -self.operator_joystick.getY(0)
if self.down_button.get():
self.down_mode()
elif self.up_button.get():
self.up_mode()
joystick_val = -self.operator_joystick.getRawAxis(1)
if joystick_val > 0.2:
joystick_val -= 0.2
elif joystick_val < -0.2:
joystick_val += 0.2
else:
joystick_val = 0.0
self.elevator.move_setpoint(joystick_val / 50.0 * 20)
if self.right_bumper_button.get():
self.intake.wrist_down()
self.intake.intake()
self.intake.open_arm()
if self.intake.has_cube():
self.rumbling = True
else:
self.intake.close_arm()
if self.left_bumper_button.get():
self.climber.set_speed(-self.operator_joystick.getRawAxis(5))
else:
wrist_speed = self.operator_joystick.getRawAxis(5)
wrist_speed = 0 if abs(wrist_speed) < 0.2 else wrist_speed
self.intake.move_wrist_setpoint(wrist_speed)
if self.has_cube_button.get():
self.intake.toggle_has_cube()
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kRightRumble, 1 if self.rumbling else 0)
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kLeftRumble, 1 if self.rumbling else 0)
self.rumbling = False
def disabledPeriodic(self):
self.drivetrain._update_odometry()
self.elevator.reset_position()
def testPeriodic(self):
self.wrist_motor.set(self.operator_joystick.getRawAxis(5) * 0.6)
self.elevator_winch.set(self.operator_joystick.getRawAxis(1))
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
self.left_drive_motor.set(0)
self.right_drive_motor.set(0)