def diagnosticsToSmartDash(self):
# Add position, velocity, and angle values to the SmartDash.
SmartDashboard.putNumber(
"Left Encoder",
self.getLeftPosition() / robotMap.countsPerRevolution)
SmartDashboard.putNumber(
"Right Encoder",
self.getRightPosition() / robotMap.countsPerRevolution)
# SmartDashboard.putNumber("Left Velocity", self.getLeftVelocity())
# SmartDashboard.putNumber("Right Velocity", self.getRightVelocity())
if RobotBase.isReal():
SmartDashboard.putNumber("Left Speed",
self.l1.getMotorOutputPercent())
SmartDashboard.putNumber("Right Speed",
self.r1.getMotorOutputPercent())
SmartDashboard.putNumber("Gyro Angle", self.gyro.getAngle())
SmartDashboard.putNumber("Barometric Pressure",
self.gyro.getBarometricPressure())
def __init__(self):
super().__init__()
# Initialize and calibrate the NavX-MXP.
self.gyro = AHRS.create_spi(wpilib.SPI.Port.kMXP)
self.gyro.reset()
# Initialize motors.
self.l1 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.left1)
self.l2 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.left2)
self.r1 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.right1)
self.r2 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.right2)
# Select a sensor for PID.
self.l1.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotMap.ctreTimeout)
self.r1.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotMap.ctreTimeout)
# Left sensor runs in reverse so the phase must be set for PID.
self.l1.setSensorPhase(True)
self.r1.setSensorPhase(True)
# Invert motor output as necessary.
self.r1.setInverted(True)
self.r2.setInverted(True)
# Set secondary motors to follow primary motor speed.
self.l2.follow(self.l1)
self.r2.follow(self.r1)
# Set talons to brake automatically.
self.l1.setNeutralMode(NeutralMode.Brake)
self.l2.setNeutralMode(NeutralMode.Brake)
self.r1.setNeutralMode(NeutralMode.Brake)
self.r2.setNeutralMode(NeutralMode.Brake)
# If code is running on a RoboRio, configure current limiting.
if RobotBase.isReal():
self.l1.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.l1.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.l1.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.l1.enableCurrentLimit(True)
self.l2.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.l2.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.l2.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.l2.enableCurrentLimit(True)
self.r1.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.r1.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.r1.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.r1.enableCurrentLimit(True)
self.r2.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.r2.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.r2.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.r2.enableCurrentLimit(True)
# Reset max recorded velocities
self.maxRecordedLeftVelocity = 0
self.maxRecordedRightVelocity = 0
self.r1.configNeutralDeadband(0.1, 10)
self.r2.configNeutralDeadband(0.1, 10)
self.l1.configNeutralDeadband(0.1, 10)
self.l2.configNeutralDeadband(0.1, 10)