Work from build 2/28

src/main/java/frc/robot/RobotContainer.java

@@ -42,14 +42,14 @@ import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-    private PhotonVision vision;
+    //private PhotonVision vision;
     private Drivetrain drivetrain;
     private Hood hood;
     private Shooter shooter;
     private IntakePivot intakePivot;
     private IntakeRoller intakeRoller;
     private Spindexer spindexer;
-    private Climber climber;
+    //private Climber climber;
 
     private CommandXboxController driver;
     private CommandXboxController secondary;
@@ -59,22 +59,23 @@ public class RobotContainer {
     private Timer shiftTimer;
 
     public RobotContainer() {
-        vision = new PhotonVision();
+        //vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
         hood = new Hood();
         shooter = new Shooter();
         intakePivot = new IntakePivot();
         intakeRoller = new IntakeRoller();
         spindexer = new Spindexer();
-        climber = new Climber();
+        //climber = new Climber();
 
+        /*
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
                 vp.visualPose()
             );
-        });
+        });*/
 
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -82,7 +83,8 @@ public class RobotContainer {
         shiftTimer = new Timer();
         shiftTimer.reset();
 
-        configureBindings();
+        //configureBindings();
+        testConfigureBindings();
         configureShiftDisplay();
 
         if(AutoConstants.kAutoConfigOk) {
@@ -106,7 +108,8 @@ public class RobotContainer {
         // This should just work, if it doesn't it's likely modules aren't assigned the right IDs
         // after the electronics rebuild. For testing normal operation nothing about the Drivetrain
         // class should need to change
-        drivetrain.setDefaultCommand(
+        drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
+        /*drivetrain.setDefaultCommand(
             drivetrain.drive(
                 driver::getLeftY,
                 driver::getLeftX, 
@@ -138,7 +141,7 @@ public class RobotContainer {
                 driver::getLeftY, 
                 driver::getLeftX
             )
-        );
+        );*/
 
         // Stop everything by default other than the drivetrain
         shooter.setDefaultCommand(shooter.stop());
@@ -146,7 +149,7 @@ public class RobotContainer {
         intakeRoller.setDefaultCommand(intakeRoller.stop());
         hood.setDefaultCommand(hood.stop());
         spindexer.setDefaultCommand(spindexer.stop());
-        climber.setDefaultCommand(climber.stop());
+        //climber.setDefaultCommand(climber.stop());
 
         // While holding POV up of the driver controller, the climber
         // should move such that its motor moves the climber down with the left
@@ -154,9 +157,9 @@ public class RobotContainer {
         // trigger axis.
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted
         // from the constants file for the subsystem having the problem. 
-        driver.povUp().whileTrue(climber.manualSpeed(() -> {
+        //driver.povUp().whileTrue(climber.manualSpeed(() -> {
-            return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
+        //    return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
-        }));
+        //}));
 
         // While holding the right bumper of the driver controller, the intake rollers
         // and the spindexer and feeder should move such that all motors are moving in such a way
@@ -165,7 +168,8 @@ public class RobotContainer {
         // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
         // constants file for the subsystem having the problem
         driver.rightBumper().whileTrue(
-            intakeRoller.runIn().alongWith(spindexer.spinToShooter())
+            //intakeRoller.runIn().alongWith(spindexer.spinToShooter())
+            spindexer.spinToShooter()
         );
 
         // While holding the left bumper of the driver controller, the intake rollers
@@ -244,6 +248,7 @@ public class RobotContainer {
             ) 
         );
 
+        /*
         driver.b().whileTrue(
             drivetrain.lockToYaw(
                 () -> {
@@ -254,7 +259,7 @@ public class RobotContainer {
                 driver::getLeftY, 
                 driver::getLeftX
             )
-        );
+        );*/
 
         shooter.setDefaultCommand(
             shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -20,15 +20,15 @@ public class DrivetrainConstants {
     public static final double kRearLeftMagEncoderOffset = 3.761;
     public static final double kRearRightMagEncoderOffset = 2.573;
 
-    public static final int kFrontLeftDrivingCANID = 0;
+    public static final int kFrontLeftDrivingCANID = 4;
     public static final int kFrontRightDrivingCANID = 3;
     public static final int kRearLeftDrivingCANID = 1;
     public static final int kRearRightDrivingCANID = 2;
 
-    public static final int kFrontLeftTurningCANID = 8;
+    public static final int kFrontLeftTurningCANID = 7; // 8
-    public static final int kFrontRightTurningCANID = 9;
+    public static final int kFrontRightTurningCANID = 21; //9
-    public static final int kRearLeftTurningCANID = 7;
+    public static final int kRearLeftTurningCANID = 6; //7
-    public static final int kRearRightTurningCANID = 6;
+    public static final int kRearRightTurningCANID = 8; //6
 
     public static final int kFrontLeftAnalogInPort = 3;
     public static final int kFrontRightAnalogInPort = 2;

src/main/java/frc/robot/constants/HoodConstants.java

@@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.Filesystem;
 
 public class HoodConstants {
     // TODO Real Values
-    public static final int kMotorCANID = 0;
+    public static final int kMotorCANID = 12;
 
     public static final double kConversionFactor = 3.0*147.0/8.0;
 
@@ -26,7 +26,7 @@ public class HoodConstants {
     public static final double kV = 0;
     public static final double kA = 0;
     public static final double kStartupAngle = 0;
-    public static final double kMaxManualSpeedMultiplier = .5;
+    public static final double kMaxManualSpeedMultiplier = 1;
 
     public static final double kAmpsToTriggerPositionReset = 10;
 

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -20,8 +20,8 @@ public class IntakePivotConstants {
         }
     }
 
-    public static final int kLeftMotorCANID = 0;
+    public static final int kLeftMotorCANID = 16;
-    public static final int kRightMotorCANID = 0;
+    public static final int kRightMotorCANID = 9;
 
     public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
 
@@ -40,7 +40,7 @@ public class IntakePivotConstants {
 
     public static final int kCurrentLimit = 30;
 
-    public static final IdleMode kIdleMode = IdleMode.kBrake;
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
     

src/main/java/frc/robot/constants/IntakeRollerConstants.java

@@ -5,30 +5,24 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 public class IntakeRollerConstants {
     // TODO Real values
-    public static final int kLeftMotorCANID = 0;
+    public static final int kMotorCANID = 20;
-    public static final int kRightMotorCANID = 0;
 
     public static final int kCurrentLimit = 40;
 
-    public static final boolean kInvertMotors = false;
+    public static final boolean kInvertMotors = true;
+
+    public static final double kSpeed = .6;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
 
     static {
         leftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kInvertMotors);
-
-        rightMotorConfig
-            .idleMode(kIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
-            .follow(kLeftMotorCANID);
     }
 }

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units;
 public class ShooterConstants {
     public enum ShooterSpeeds {
         kHubSpeed(0),
-        kFeedSpeed(0);
+        kFeedSpeed(0.35);
 
         private double speedMPS;
 
@@ -27,30 +27,28 @@ public class ShooterConstants {
     public static final double kWheelDiameter = Units.inchesToMeters(4);
 
     // TODO Real values
-    public static final int kLeftShooterMotor1CANID = 0;
+    public static final int kLeftShooterMotorCANID = 2;
-    public static final int kLeftShooterMotor2CANID = 0;
+    public static final int kRightShooterMotorCANID = 5;
-    public static final int kRightShooterMotor1CANID = 0;
-    public static final int kRightShooterMotor2CANID = 0;
 
-    public static final boolean kLeftShooterMotor1Inverted = false;
+    public static final boolean kLeftShooterMotorInverted = true;
-    public static final boolean kLeftShooterMotor2Inverted = false;
+    public static final boolean kRightShooterMotorInverted = false;
-    public static final boolean kRightShooterMotor1Inverted = false;
-    public static final boolean kRightShooterMotor2Inverted = false;
 
-    public static final double kLeftP = 0;
+    public static final double kLeftP = 0.1;
     public static final double kLeftI = 0;
     public static final double kLeftD = 0;
     public static final double kLeftS = 0;
     public static final double kLeftV = 0.21;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0;
+    public static final double kRightP = 0.1;
     public static final double kRightI = 0;
     public static final double kRightD = 0;
     public static final double kRightS = 0;
     public static final double kRightV = 0.21;
     public static final double kRightA = 0;
 
+    public static final double kMaxManualSpeedMultiplier = 1;
+
     public static final double kShooterHeightMeters = 0;
 
     // TODO Is this value sane?
@@ -60,51 +58,37 @@ public class ShooterConstants {
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
-    public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig();
 
 
     static {
-        kLeftMotor1Config
+        kLeftMotorConfig
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kLeftShooterMotor1Inverted);
+            .inverted(kLeftShooterMotorInverted);
-        kLeftMotor1Config.absoluteEncoder
+        kLeftMotorConfig.absoluteEncoder
             .positionConversionFactor(kWheelDiameter * Math.PI)
             .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kLeftMotor1Config.closedLoop
+        kLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kLeftP, kLeftI, kLeftD)
             .outputRange(-1, 1)
             .feedForward
                 .sva(kLeftS, kLeftV, kLeftA);
 
-        kLeftMotor2Config
+        kRightMotorConfig
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kLeftShooterMotor2Inverted)
+            .inverted(kRightShooterMotorInverted);
-            .follow(kLeftShooterMotor1CANID);
+        kRightMotorConfig.absoluteEncoder
-
-        kRightMotor1Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRightShooterMotor1Inverted);
-        kRightMotor1Config.absoluteEncoder
             .positionConversionFactor(kWheelDiameter * Math.PI)
             .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kRightMotor1Config.closedLoop
+        kRightMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
             .feedForward
                 .sva(kRightS, kRightV, kRightA);
-
-        kRightMotor2Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRightShooterMotor2Inverted)
-            .follow(kRightShooterMotor1CANID);
     }
 }

src/main/java/frc/robot/constants/SpindexerConstants.java

@@ -10,16 +10,19 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 public class SpindexerConstants {
     // TODO Real values
     public static final int kSpindexerMotorCANID = 0;
-    public static final int kFeederMotorCANID = 0;
+    public static final int kFeederMotorCANID = 4;
 
-    public static final int kSpindexerStatorCurrentLimit = 90;
+    public static final int kSpindexerStatorCurrentLimit = 110;
-    public static final int kSpindexerSupplyCurrentLimit = 50;
+    public static final int kSpindexerSupplyCurrentLimit = 60;
     public static final int kFeederCurrentLimit = 40;
 
-    public static final boolean kFeederMotorInverted = false;
+    public static final double kSpindexerSpeed = 1;
+    public static final double kFeederSpeed = 1;
+
+    public static final boolean kFeederMotorInverted = true;
 
     public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
-    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake;
+    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
 
     public static final IdleMode kFeederIdleMode = IdleMode.kBrake;
 

src/main/java/frc/robot/subsystems/Hood.java

@@ -4,7 +4,6 @@ import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;

src/main/java/frc/robot/subsystems/IntakeRoller.java

@@ -10,41 +10,33 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeRollerConstants;
 
 public class IntakeRoller extends SubsystemBase {
-    private SparkMax leftMotor;
+    private SparkMax motor;
-    private SparkMax rightMotor;
 
     public IntakeRoller() {
-        leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
+        motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
-        rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
 
-        leftMotor.configure(
+        motor.configure(
             IntakeRollerConstants.leftMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
-
-        rightMotor.configure(
-            IntakeRollerConstants.rightMotorConfig, 
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
     }
 
     public Command runIn() {
         return run(() -> {
-            leftMotor.set(1);
+            motor.set(IntakeRollerConstants.kSpeed);
         });
     }
 
     public Command runOut() {
         return run(() -> {
-            leftMotor.set(-1);
+            motor.set(-IntakeRollerConstants.kSpeed);
         });
     }
 
     public Command stop() {
         return run(() -> {
-            leftMotor.set(0);
+            motor.set(0);
         });
     }
     

src/main/java/frc/robot/subsystems/PhotonVision.java

@@ -75,19 +75,19 @@ public class PhotonVision extends SubsystemBase {
 
             if(!results.isEmpty()) {
                 latestResults.set(i, results.get(results.size() - 1));
-            }
 
-            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
+                Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
 
-            if(!pose.isEmpty()) {
+                if(!pose.isEmpty()) {
-                VisualPose visualPose = new VisualPose(
+                    VisualPose visualPose = new VisualPose(
-                    cameras[i].getName(),
+                        cameras[i].getName(),
-                    pose.get().estimatedPose.toPose2d(), 
+                        pose.get().estimatedPose.toPose2d(), 
-                    pose.get().timestampSeconds
+                        pose.get().timestampSeconds
-                );
+                    );
 
-                for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                    for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
-                    consumer.accept(visualPose);
+                        consumer.accept(visualPose);
+                    }
                 }
             }
         }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -19,10 +19,8 @@ import frc.robot.constants.ShooterConstants;
 import frc.robot.constants.ShooterConstants.ShooterSpeeds;
 
 public class Shooter extends SubsystemBase {
-    private SparkMax leftMotor1;
+    private SparkMax leftMotor;
-    private SparkMax leftMotor2;
+    private SparkMax rightMotor;
-    private SparkMax rightMotor1;
-    private SparkMax rightMotor2;
 
     private AbsoluteEncoder leftEncoder;
     private AbsoluteEncoder rightEncoder;
@@ -33,40 +31,26 @@ public class Shooter extends SubsystemBase {
     private ShooterSpeeds targetSpeeds;
 
     public Shooter() {
-        leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless);
+        leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
-        leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(ShooterConstants.kRightShooterMotorCANID, MotorType.kBrushless);
-        rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless);
-        rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless);
 
-        leftMotor1.configure(
+        leftMotor.configure(
-            ShooterConstants.kLeftMotor1Config, 
+            ShooterConstants.kLeftMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        rightMotor1.configure(
+        rightMotor.configure(
-            ShooterConstants.kRightMotor1Config, 
+            ShooterConstants.kRightMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        leftMotor2.configure(
+        leftEncoder = leftMotor.getAbsoluteEncoder();
-            ShooterConstants.kLeftMotor2Config, 
+        rightEncoder = rightMotor.getAbsoluteEncoder();
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
 
-        rightMotor2.configure(
+        leftClosedLoopController = leftMotor.getClosedLoopController();
-            ShooterConstants.kRightMotor2Config, 
+        rightClosedLoopController = rightMotor.getClosedLoopController();
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        leftEncoder = leftMotor1.getAbsoluteEncoder();
-        rightEncoder = rightMotor1.getAbsoluteEncoder();
-
-        leftClosedLoopController = leftMotor1.getClosedLoopController();
-        rightClosedLoopController = rightMotor1.getClosedLoopController();
 
         // TODO Set this to the initial startup speed
         targetSpeeds = null;
@@ -92,8 +76,8 @@ public class Shooter extends SubsystemBase {
 
         return run(() -> {
             if(targetSpeeds == null) {
-                leftMotor1.disable();
+                leftMotor.disable();
-                rightMotor1.disable();
+                rightMotor.disable();
             } else {
                 leftClosedLoopController.setSetpoint(
                     targetSpeeds.getSpeedMPS(), 
@@ -101,7 +85,7 @@ public class Shooter extends SubsystemBase {
                 );
 
                 rightClosedLoopController.setSetpoint(
-                    targetSpeeds.getSpeedMPS(), 
+                    -targetSpeeds.getSpeedMPS(), 
                     ControlType.kVelocity
                 );
             }
@@ -112,8 +96,8 @@ public class Shooter extends SubsystemBase {
         targetSpeeds = null;
 
         return run(() -> {
-            leftMotor1.set(speed.getAsDouble());
+            leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
-            rightMotor1.set(speed.getAsDouble());
+            rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
         });
     }
 

src/main/java/frc/robot/subsystems/Spindexer.java

@@ -37,15 +37,19 @@ public class Spindexer extends SubsystemBase {
 
     public Command spinToShooter() {
         return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(1));
+            spindexerMotor.setControl(
-            feederMotor.set(1);
+                spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.set(SpindexerConstants.kFeederSpeed);
         });
     }
 
     public Command spinToIntake() {
         return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1));
+            spindexerMotor.setControl(
-            feederMotor.set(-1);
+                spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.set(-SpindexerConstants.kFeederSpeed);
         });
     }