Merge branch 'master' into usb_vision

Author: mvande

src/main/java/frc/robot/Constants.java

@@ -318,9 +318,10 @@ public class CanIDs {
     public static final int INTAKE_FRONT_ROLLERS_CAN_ID = 8;
     public static final int INTAKE_BACK_ROLLERS_CAN_ID = 9;
     public static final int INTAKE_EXTENDER_CAN_ID = 10;
-    public static final int HOPPER_CAN_ID = 11;
-    public static final int SHOOTER_LEAD_CAN_ID = 12;
-    public static final int SHOOTER_FOLLOW_CAN_ID = 13;
+    public static final int HOPPER_LEAD_CAN_ID = 11;
+    public static final int HOPPER_FOLLOW_CAN_ID = 12;
+    public static final int SHOOTER_LEAD_CAN_ID = 13;
+    public static final int SHOOTER_FOLLOW_CAN_ID = 14;
   }
 
   public class FieldConstants {

src/main/java/frc/robot/subsystems/hopper/Hopper.java

@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems.hopper;
 
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -17,6 +19,8 @@
 import frc.robot.Constants;
 import frc.robot.Constants.HopperConstants;
 import frc.robot.Constants.Mode;
+import frc.robot.Constants.ShooterConstants;
+
 import org.littletonrobotics.junction.Logger;
 
 public class Hopper extends SubsystemBase {

src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java

@@ -13,34 +13,45 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.Constants;
 import frc.robot.Constants.HopperConstants;
+import frc.robot.Constants.ShooterConstants;
 
 public class HopperIOReal implements HopperIO {
 
-  private final SparkFlex motor =
-      new SparkFlex(Constants.CanIDs.HOPPER_CAN_ID, MotorType.kBrushless);
+  private final SparkFlex leadMotor =
+      new SparkFlex(Constants.CanIDs.HOPPER_LEAD_CAN_ID, MotorType.kBrushless);
+  private final SparkFlex followMotor =
+      new SparkFlex(Constants.CanIDs.HOPPER_FOLLOW_CAN_ID, MotorType.kBrushless);
   private SparkFlexConfig config = Constants.ElevatorConstants.MOTOR_CONFIG();
 
-  private final RelativeEncoder encoder = motor.getEncoder();
+  private final RelativeEncoder encoder = leadMotor.getEncoder();
+
+  private final SparkFlexConfig followConfig = new SparkFlexConfig();
 
   // private final VL6180 timeOfFlight = new VL6180(Port.kOnboard);
 
+  public HopperIOReal(){
+    followConfig.follow(leadMotor, ShooterConstants.FOLLOW_INVERT);
+    followMotor.configure(
+        followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+  }
+
   @Override
   public void updateInputs(HopperInputs inputs) {
     inputs.velocityRPM = encoder.getVelocity();
-    inputs.appliedOutput = motor.getAppliedOutput();
-    inputs.currentAmps = motor.getOutputCurrent();
-    inputs.tempCelsius = motor.getMotorTemperature();
+    inputs.appliedOutput = leadMotor.getAppliedOutput();
+    inputs.currentAmps = leadMotor.getOutputCurrent();
+    inputs.tempCelsius = leadMotor.getMotorTemperature();
     // inputs.tofDistanceInches = timeOfFlight.getDistance().in(Units.Inches);
   }
 
   @Override
   public void setVoltage(double volts) {
-    motor.setVoltage(volts);
+    leadMotor.setVoltage(volts);
   }
 
   @Override
   public void setVel(AngularVelocity angle) {
-    motor
+    leadMotor
         .getClosedLoopController()
         .setReference(
             angle.in(Units.RPM) / HopperConstants.GEAR_RATIO,
@@ -51,13 +62,15 @@ public void setVel(AngularVelocity angle) {
   public void configMotor(double kV, double kP, double maxAcceleration) {
     config.closedLoop.pidf(kP, 0, 0, kV);
     config.closedLoop.maxMotion.maxAcceleration(maxAcceleration);
-    motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    leadMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
   }
 
   @Override
   public boolean setIdleMode(IdleMode value) {
     config.idleMode(value);
-    return motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
+    followConfig.idleMode(value);
+    return leadMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
+        == REVLibError.kOk && followMotor.configure(followConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)
         == REVLibError.kOk;
   }
 }