diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index ff65545..397acb0 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -1,5 +1,7 @@ package org.team340.robot; +import com.revrobotics.CANSparkBase.ExternalFollower; +import com.revrobotics.CANSparkBase.IdleMode; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; @@ -9,7 +11,11 @@ import org.team340.lib.swerve.SwerveBase.SwerveMotorType; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; +import org.team340.lib.util.Math2; import org.team340.lib.util.config.PIDConfig; +import org.team340.lib.util.config.rev.AbsoluteEncoderConfig; +import org.team340.lib.util.config.rev.SparkMaxConfig; +import org.team340.lib.util.config.rev.SparkPIDControllerConfig; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -72,8 +78,10 @@ public static final class RobotMap { public static final int FRONT_RIGHT_MOVE = 8; public static final int FRONT_RIGHT_TURN = 9; - public static final int INTAKE_DEPLOY_MOTOR = 20; - public static final int INTAKE_ROLLER_MOTOR = 21; + public static final int INTAKE_PIVOT_LEFT_MOTOR = 20; + public static final int INTAKE_PIVOT_RIGHT_MOTOR = 21; + public static final int INTAKE_ROLLER_UPPER_MOTOR = 22; + public static final int INTAKE_ROLLER_LOWER_MOTOR = 23; public static final int SHOOTER_PIVOT_MOTOR = 30; public static final int SHOOTER_FEED_MOTOR = 31; @@ -85,54 +93,88 @@ public static final class RobotMap { public static final class IntakeConstants { - public static final PIDConfig INTAKE_DEPLOY_STRONG_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); - public static final PIDConfig INTAKE_DEPLOY_WEAK_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); - - public static final double INTAKE_DEPLOY_POSITION = 0.0; - public static final double INTAKE_DEPLOY_POSITION_TOLERANCE = 0.0; - - public static final double INTAKE_DEPLOY_ROLLER_SPEED = 0.0; - public static final double INTAKE_SPIT_ROLLER_SPEED = -0.0; + private static final SparkMaxConfig PIVOT_MOTOR_BASE_CONFIG = new SparkMaxConfig() + .clearFaults() + .restoreFactoryDefaults() + .enableVoltageCompensation(VOLTAGE) + .setSmartCurrentLimit(30) + .setIdleMode(IdleMode.kBrake) + .setClosedLoopRampRate(1.5) + .setOpenLoopRampRate(1.5); + private static final SparkMaxConfig ROLLER_MOTOR_BASE_CONFIG = new SparkMaxConfig() + .clearFaults() + .restoreFactoryDefaults() + .enableVoltageCompensation(VOLTAGE) + .setSmartCurrentLimit(30) + .setIdleMode(IdleMode.kCoast) + .setClosedLoopRampRate(1.5) + .setOpenLoopRampRate(1.5); + + public static final SparkMaxConfig PIVOT_LEFT_MOTOR_CONFIG = PIVOT_MOTOR_BASE_CONFIG.clone().setInverted(false); + public static final SparkMaxConfig PIVOT_RIGHT_MOTOR_CONFIG = PIVOT_MOTOR_BASE_CONFIG + .clone() + .follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_PIVOT_LEFT_MOTOR, false); + public static final SparkMaxConfig ROLLER_UPPER_MOTOR_CONFIG = ROLLER_MOTOR_BASE_CONFIG.clone().setInverted(false); + public static final SparkMaxConfig ROLLER_LOWER_MOTOR_CONFIG = ROLLER_MOTOR_BASE_CONFIG + .clone() + .follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_ROLLER_UPPER_MOTOR, true); + + public static final int PIVOT_WEAK_PID_SLOT = 0; + public static final int PIVOT_STRONG_PID_SLOT = 1; + + public static final SparkPIDControllerConfig PIVOT_MOTOR_PID_CONFIG = new SparkPIDControllerConfig() + .setPID(0.0, 0.0, 0.0, PIVOT_WEAK_PID_SLOT) + .setPID(0.0, 0.0, 0.0, PIVOT_STRONG_PID_SLOT); + + public static final AbsoluteEncoderConfig PIVOT_ENCODER_CONFIG = new AbsoluteEncoderConfig() + .setZeroOffset(0.0) + .setInverted(false) + .setPositionConversionFactor(Math2.TWO_PI) + .setVelocityConversionFactor(Math2.TWO_PI / 60.0); + + public static final double DEPLOY_POSITION = 0.0; + public static final double DEPLOY_POSITION_TOLERANCE = 0.0; + + public static final double DEPLOY_ROLLER_SPEED = 0.0; + public static final double SPIT_ROLLER_SPEED = -0.0; } public static final class ShooterConstants { public static final double FEED_INTAKE_SPEED = 0.0; - public static final PIDConfig SHOOTER_FEED_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); - public static final PIDConfig SHOOTER_LEFT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); - public static final PIDConfig SHOOTER_RIGHT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); + public static final PIDConfig FEED_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); + public static final PIDConfig LEFT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); + public static final PIDConfig RIGHT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); /** * All distances in inches. All angles in radians. */ public static final class PivotConstants { - public static final double PIVOT_REL_ENC_CONVERSION = 0.2; + public static final double REL_ENC_CONVERSION = 0.2; - public static final double PIVOT_SHOOTER_LENGTH = 0.0; - public static final double PIVOT_BASE_LENGTH = 0.0; - public static final double PIVOT_MINIMUM_LENGTH_OF_DART = 0.0; - public static final double SUM_OF_SQUARES_OF_LENGTHS = - PIVOT_SHOOTER_LENGTH * PIVOT_SHOOTER_LENGTH + PIVOT_BASE_LENGTH * PIVOT_BASE_LENGTH; + public static final double SHOOTER_LENGTH = 0.0; + public static final double BASE_LENGTH = 0.0; + public static final double MINIMUM_LENGTH_OF_DART = 0.0; + public static final double SUM_OF_SQUARES_OF_LENGTHS = SHOOTER_LENGTH * SHOOTER_LENGTH + BASE_LENGTH * BASE_LENGTH; - public static final double PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS = 2 * PIVOT_SHOOTER_LENGTH * PIVOT_BASE_LENGTH; + public static final double TWICE_THE_PRODUCT_OF_LENGTHS = 2 * SHOOTER_LENGTH * BASE_LENGTH; - public static final double PIVOT_OFFSET_ANGLE = Math.acos( - (SUM_OF_SQUARES_OF_LENGTHS - PIVOT_MINIMUM_LENGTH_OF_DART * PIVOT_MINIMUM_LENGTH_OF_DART) / - PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS + public static final double OFFSET_ANGLE = Math.acos( + (SUM_OF_SQUARES_OF_LENGTHS - MINIMUM_LENGTH_OF_DART * MINIMUM_LENGTH_OF_DART) / TWICE_THE_PRODUCT_OF_LENGTHS ); - public static final double PIVOT_MINIMUM_ANGLE = 0.0; - public static final double PIVOT_MAXIMUM_ANGLE = 0.0; + public static final double MINIMUM_ANGLE = 0.0; + public static final double MAXIMUM_ANGLE = 0.0; - public static final PIDConfig PIVOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); - public static final double PIVOT_PID_MIN_OUTPUT = 0.0; - public static final double PIVOT_PID_MAX_OUTPUT = 0.0; - public static final double PIVOT_MIN_VEL = 0.0; - public static final double PIVOT_MAX_VEL = 0.0; - public static final double PIVOT_MAX_ACCEL = 0.0; - public static final double PIVOT_CLOSED_LOOP_ERR = 0.0; + public static final PIDConfig PID = new PIDConfig(0.0, 0.0, 0.0, 0.0); + public static final double PID_MIN_OUTPUT = 0.0; + public static final double PID_MAX_OUTPUT = 0.0; + public static final double MIN_VEL = 0.0; + public static final double MAX_VEL = 0.0; + public static final double MAX_ACCEL = 0.0; + public static final double CLOSED_LOOP_ERR = 0.0; } } diff --git a/src/main/java/org/team340/robot/subsystems/Intake.java b/src/main/java/org/team340/robot/subsystems/Intake.java index 8624b58..327b676 100644 --- a/src/main/java/org/team340/robot/subsystems/Intake.java +++ b/src/main/java/org/team340/robot/subsystems/Intake.java @@ -3,6 +3,8 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj2.command.Command; import org.team340.lib.GRRSubsystem; @@ -16,48 +18,52 @@ */ public class Intake extends GRRSubsystem { - private final CANSparkMax deployMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_DEPLOY_MOTOR, MotorType.kBrushless); - private final CANSparkMax rollerMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_MOTOR, MotorType.kBrushless); + private final CANSparkMax pivotLeftMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_PIVOT_LEFT_MOTOR, MotorType.kBrushless); + private final CANSparkMax pivotRightMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_PIVOT_RIGHT_MOTOR, MotorType.kBrushless); + private final CANSparkMax rollerUpperMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_UPPER_MOTOR, MotorType.kBrushless); + private final CANSparkMax rollerLowerMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_LOWER_MOTOR, MotorType.kBrushless); - private final SparkPIDController deployPID = deployMotor.getPIDController(); + private final SparkPIDController pivotPID = pivotLeftMotor.getPIDController(); + private final SparkAbsoluteEncoder pivotEncoder = createSparkMaxAbsoluteEncoder("Pivot Encoder", pivotLeftMotor, Type.kDutyCycle); public Intake() { super("Intake"); - deployPID.setP(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.p(), 0); - deployPID.setI(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.i(), 0); - deployPID.setD(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.d(), 0); + IntakeConstants.PIVOT_LEFT_MOTOR_CONFIG.apply(pivotLeftMotor); + IntakeConstants.PIVOT_RIGHT_MOTOR_CONFIG.apply(pivotRightMotor); + IntakeConstants.ROLLER_UPPER_MOTOR_CONFIG.apply(rollerUpperMotor); + IntakeConstants.ROLLER_LOWER_MOTOR_CONFIG.apply(rollerLowerMotor); - deployPID.setP(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.p(), 1); - deployPID.setI(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.i(), 1); - deployPID.setD(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.d(), 1); + IntakeConstants.PIVOT_MOTOR_PID_CONFIG.apply(pivotLeftMotor, pivotPID); + + IntakeConstants.PIVOT_ENCODER_CONFIG.apply(pivotLeftMotor, pivotEncoder); } public Command deploy() { Mutable weakPID = new Mutable<>(false); return commandBuilder("intake.deploy()") - .onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_DEPLOY_ROLLER_SPEED)) + .onInitialize(() -> rollerUpperMotor.set(IntakeConstants.DEPLOY_ROLLER_SPEED)) .onExecute(() -> { if ( Math2.epsilonEquals( - IntakeConstants.INTAKE_DEPLOY_POSITION, - deployMotor.getEncoder().getPosition(), - IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE + IntakeConstants.DEPLOY_POSITION, + pivotLeftMotor.getEncoder().getPosition(), + IntakeConstants.DEPLOY_POSITION_TOLERANCE ) ) weakPID.set(true); - deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0); + pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0); }) .onEnd(() -> { - rollerMotor.stopMotor(); - deployMotor.stopMotor(); + rollerUpperMotor.stopMotor(); + pivotLeftMotor.stopMotor(); }); } public Command retract() { return commandBuilder("intake.retract()") - .onInitialize(() -> rollerMotor.stopMotor()) + .onInitialize(() -> rollerUpperMotor.stopMotor()) .onExecute(() -> { - deployPID.setReference(0, ControlType.kPosition, 0); + pivotPID.setReference(0, ControlType.kPosition, 0); }); } @@ -65,21 +71,21 @@ public Command spit() { Mutable weakPID = new Mutable<>(false); return commandBuilder("intake.spit()") - .onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_SPIT_ROLLER_SPEED)) + .onInitialize(() -> rollerUpperMotor.set(IntakeConstants.SPIT_ROLLER_SPEED)) .onExecute(() -> { if ( Math2.epsilonEquals( - IntakeConstants.INTAKE_DEPLOY_POSITION, - deployMotor.getEncoder().getPosition(), - IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE + IntakeConstants.DEPLOY_POSITION, + pivotLeftMotor.getEncoder().getPosition(), + IntakeConstants.DEPLOY_POSITION_TOLERANCE ) ) weakPID.set(true); - deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0); + pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0); }) .onEnd(() -> { - rollerMotor.stopMotor(); - deployMotor.stopMotor(); + rollerUpperMotor.stopMotor(); + pivotLeftMotor.stopMotor(); }); } } diff --git a/src/main/java/org/team340/robot/subsystems/Shooter.java b/src/main/java/org/team340/robot/subsystems/Shooter.java index cb7db78..147c05b 100644 --- a/src/main/java/org/team340/robot/subsystems/Shooter.java +++ b/src/main/java/org/team340/robot/subsystems/Shooter.java @@ -52,41 +52,33 @@ public class Shooter extends GRRSubsystem { public Shooter() { super("Shooter"); - pivotRelativeEncoder.setPositionConversionFactor(PivotConstants.PIVOT_REL_ENC_CONVERSION); + pivotRelativeEncoder.setPositionConversionFactor(PivotConstants.REL_ENC_CONVERSION); pivotAbsoluteEncoder.setPositionConversionFactor(Math2.TWO_PI); new SparkPIDControllerConfig() - .setPID(PivotConstants.PIVOT_PID.p(), PivotConstants.PIVOT_PID.i(), PivotConstants.PIVOT_PID.d()) - .setIZone(PivotConstants.PIVOT_PID.iZone()) - .setOutputRange(PivotConstants.PIVOT_PID_MIN_OUTPUT, PivotConstants.PIVOT_PID_MAX_OUTPUT) + .setPID(PivotConstants.PID.p(), PivotConstants.PID.i(), PivotConstants.PID.d()) + .setIZone(PivotConstants.PID.iZone()) + .setOutputRange(PivotConstants.PID_MIN_OUTPUT, PivotConstants.PID_MAX_OUTPUT) .apply(pivotMotor, pivotPID); - pivotPID.setSmartMotionMaxVelocity(PivotConstants.PIVOT_MAX_VEL, 0); - pivotPID.setSmartMotionMaxVelocity(PivotConstants.PIVOT_MAX_VEL, 0); - pivotPID.setSmartMotionMinOutputVelocity(PivotConstants.PIVOT_MAX_VEL, 0); - pivotPID.setSmartMotionMaxAccel(PivotConstants.PIVOT_MAX_ACCEL, 0); - pivotPID.setSmartMotionAllowedClosedLoopError(PivotConstants.PIVOT_CLOSED_LOOP_ERR, 0); + pivotPID.setSmartMotionMaxVelocity(PivotConstants.MAX_VEL, 0); + pivotPID.setSmartMotionMaxVelocity(PivotConstants.MAX_VEL, 0); + pivotPID.setSmartMotionMinOutputVelocity(PivotConstants.MAX_VEL, 0); + pivotPID.setSmartMotionMaxAccel(PivotConstants.MAX_ACCEL, 0); + pivotPID.setSmartMotionAllowedClosedLoopError(PivotConstants.CLOSED_LOOP_ERR, 0); new SparkPIDControllerConfig() - .setPID(ShooterConstants.SHOOTER_FEED_PID.p(), ShooterConstants.SHOOTER_FEED_PID.i(), ShooterConstants.SHOOTER_FEED_PID.d()) - .setIZone(ShooterConstants.SHOOTER_FEED_PID.iZone()) + .setPID(ShooterConstants.FEED_PID.p(), ShooterConstants.FEED_PID.i(), ShooterConstants.FEED_PID.d()) + .setIZone(ShooterConstants.FEED_PID.iZone()) .apply(feedMotor, feedPID); new SparkPIDControllerConfig() - .setPID( - ShooterConstants.SHOOTER_LEFT_SHOOT_PID.p(), - ShooterConstants.SHOOTER_LEFT_SHOOT_PID.i(), - ShooterConstants.SHOOTER_LEFT_SHOOT_PID.d() - ) - .setIZone(ShooterConstants.SHOOTER_LEFT_SHOOT_PID.iZone()) + .setPID(ShooterConstants.LEFT_SHOOT_PID.p(), ShooterConstants.LEFT_SHOOT_PID.i(), ShooterConstants.LEFT_SHOOT_PID.d()) + .setIZone(ShooterConstants.LEFT_SHOOT_PID.iZone()) .apply(leftShootMotor, leftShootPID); new SparkPIDControllerConfig() - .setPID( - ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.p(), - ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.i(), - ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.d() - ) - .setIZone(ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.iZone()) + .setPID(ShooterConstants.RIGHT_SHOOT_PID.p(), ShooterConstants.RIGHT_SHOOT_PID.i(), ShooterConstants.RIGHT_SHOOT_PID.d()) + .setIZone(ShooterConstants.RIGHT_SHOOT_PID.iZone()) .apply(rightShootMotor, rightShootPID); } @@ -95,14 +87,14 @@ public Shooter() { * @param angleToShootAt This is the angle that will be used. */ private void setShooterToAngle(double angleToShootAt) { - if (angleToShootAt < PivotConstants.PIVOT_MINIMUM_ANGLE || angleToShootAt > PivotConstants.PIVOT_MAXIMUM_ANGLE) { + if (angleToShootAt < PivotConstants.MINIMUM_ANGLE || angleToShootAt > PivotConstants.MAXIMUM_ANGLE) { DriverStation.reportWarning( "Invalid shooter pivot angle. " + angleToShootAt + " is not between " + - PivotConstants.PIVOT_MINIMUM_ANGLE + + PivotConstants.MINIMUM_ANGLE + " and " + - PivotConstants.PIVOT_MAXIMUM_ANGLE, + PivotConstants.MAXIMUM_ANGLE, false ); return; @@ -118,10 +110,10 @@ private void setShooterToAngle(double angleToShootAt) { private double dartExtensionFromAngle(double sensorAngle) { double totalLengthOfDart = Math.sqrt( PivotConstants.SUM_OF_SQUARES_OF_LENGTHS - - PivotConstants.PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS * - Math.cos(sensorAngle + PivotConstants.PIVOT_OFFSET_ANGLE) + PivotConstants.TWICE_THE_PRODUCT_OF_LENGTHS * + Math.cos(sensorAngle + PivotConstants.OFFSET_ANGLE) ); - return (totalLengthOfDart - PivotConstants.PIVOT_MINIMUM_LENGTH_OF_DART); + return (totalLengthOfDart - PivotConstants.MINIMUM_LENGTH_OF_DART); } /**