configs.py

import math
import rev
from wpilib import RobotBase

import constants
import ids


# =============================================================================
# This file contains configuration objects for our motors. We put them all here
# because they are big and bulky and more analogous to the constants we have in
# constants.py.
# =============================================================================

driveMotorConfig = rev.SparkMaxConfig()
driveMotorConfig.smartCurrentLimit(30)
driveMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
# Convert position from rotations to m
driveMotorConfig.encoder.positionConversionFactor(math.pi * constants.wheelDiameter / constants.driveMotorReduction)
# Convert velocity from RPM to m/s
driveMotorConfig.encoder.velocityConversionFactor(
    math.pi * constants.wheelDiameter / constants.driveMotorReduction / 60
)
# Use PIDF for control with the built-in NEO encoder for feedback.
driveMotorConfig.closedLoop.setFeedbackSensor(rev.FeedbackSensor.kPrimaryEncoder)
driveMotorConfig.closedLoop.pid(0.05, 0, 0)
driveMotorConfig.closedLoop.feedForward.kV(1 / constants.physicalMaxSpeed * 12) # kV is in volts/(m/s)

steerMotorConfig = rev.SparkMaxConfig()
steerMotorConfig.smartCurrentLimit(20)
# Convert velocity (as measured by built-in relative encoder) from RPM to rad/s.
# TODO: Shouldn't this take the gear ratio of the steering gearbox into account?
steerMotorConfig.encoder.velocityConversionFactor(2 * math.pi / 60)
# Invert the absolute encoder so counter-clockwise is positive.
steerMotorConfig.absoluteEncoder.inverted(True)
# Convert position (as measured by absolute encoder) from rotations to radians.
steerMotorConfig.absoluteEncoder.positionConversionFactor(2 * math.pi)
# Convert velocity (as measured by absolute encoder) from RPM to rad/s.
steerMotorConfig.absoluteEncoder.velocityConversionFactor(2 * math.pi / 60)
# Use PID for control with the absolute encoder for feedback. Because this PID
# controller works in angles, which wrap around, we enable position wrapping.
steerMotorConfig.closedLoop.setFeedbackSensor(rev.FeedbackSensor.kAbsoluteEncoder)
if RobotBase.isSimulation():
    # A bit of a hack, but the PID values used for the real robot seem jittery in the simulator.
    # It would be nice to align the simulator behavior with real life but for now this works.
    steerMotorConfig.closedLoop.pid(1, 0, 0)
else:
    steerMotorConfig.closedLoop.pid(2, 0, 0.2)
steerMotorConfig.closedLoop.positionWrappingEnabled(True)
steerMotorConfig.closedLoop.positionWrappingInputRange(-math.pi, math.pi)

rightShooterMotorConfig = rev.SparkMaxConfig()
rightShooterMotorConfig.smartCurrentLimit(40)
rightShooterMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kCoast)
rightShooterMotorConfig.inverted(False)
rightShooterMotorConfig.encoder.positionConversionFactor(constants.shooterMotorRatio)
rightShooterMotorConfig.encoder.velocityConversionFactor(constants.shooterMotorRatio)

leftShooterMotorConfig = rev.SparkMaxConfig()
leftShooterMotorConfig.smartCurrentLimit(40)
leftShooterMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kCoast)
leftShooterMotorConfig.inverted(True)
leftShooterMotorConfig.encoder.positionConversionFactor(constants.shooterMotorRatio)
leftShooterMotorConfig.encoder.velocityConversionFactor(constants.shooterMotorRatio)

agitatorMotorConfig = rev.SparkMaxConfig()
agitatorMotorConfig.smartCurrentLimit(40)
agitatorMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
agitatorMotorConfig.inverted(True)

indexerMotorConfig = rev.SparkMaxConfig()
indexerMotorConfig.smartCurrentLimit(40)
indexerMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
indexerMotorConfig.inverted(True)

intakeMotorConfig = rev.SparkMaxConfig()
intakeMotorConfig.smartCurrentLimit(40)
intakeMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
intakeMotorConfig.encoder.positionConversionFactor(2*math.pi / constants.intakeMotorReduction)
intakeMotorConfig.encoder.velocityConversionFactor(2*math.pi / constants.intakeMotorReduction / 60)
intakeMotorConfig.closedLoop.setFeedbackSensor(rev.FeedbackSensor.kPrimaryEncoder)
intakeMotorConfig.closedLoop.pid(1, 0, 0)
intakeMotorConfig.closedLoop.outputRange(-constants.intakeSpeed, constants.intakeSpeed)
intakeMotorConfig.softLimit.forwardSoftLimit(constants.intakeOutAngle)
intakeMotorConfig.softLimit.forwardSoftLimitEnabled(True)
intakeMotorConfig.softLimit.reverseSoftLimit(constants.intakeInAngle)
intakeMotorConfig.softLimit.reverseSoftLimitEnabled(True)

intakeFollowerMotorConfig = rev.SparkMaxConfig()
intakeFollowerMotorConfig.apply(intakeMotorConfig)
intakeFollowerMotorConfig.follow(ids.leftIntake, True)

rollerMotorConfig = rev.SparkMaxConfig()
rollerMotorConfig.smartCurrentLimit(20)
rollerMotorConfig.setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)