Merge remote-tracking branch 'origin/main'

src/main/java/frc/robot/OperatorInput.java

@@ -47,7 +47,8 @@ public class OperatorInput {
   final Trigger openClaw = driver.leftStick();
   final Trigger closeClaw = driver.rightStick();
 
-  final Trigger IntakeOn = operator.a();
+  final Trigger rollersIn = operator.povRight();
+  final Trigger rollersOut = operator.povLeft();
 
   // final Trigger armbendup = operator.povUp();
   // final Trigger armbenddown = operator.povDown();

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

@@ -36,10 +36,13 @@
 import frc.robot.commands.ElevatorCommands.ManualElevatorCommand;
 import frc.robot.commands.ElevatorCommands.ResetElevatorEncoderCommand;
 import frc.robot.commands.IntakeCommands.IntakeSetPivotCommand;
+import frc.robot.commands.IntakeCommands.IntakeSetRollersCommand;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.DriveConstants;
+import frc.robot.constants.IntakeConstants;
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.IntakeSubsystem.IntakeIO;
 import frc.robot.subsystems.IntakeSubsystem.IntakeSubsystem;
 import frc.robot.subsystems.AutoSubsystem.AutoSubsystem;
 import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
@@ -114,12 +117,12 @@ public RobotContainer() {
                         new ModuleIOHybrid(3, TunerConstants.BackRight));
 
         elevatorSubsystem =
-                new ElevatorSubsystem(new ElevatorIOTalonFX(),
+                new ElevatorSubsystem(new ElevatorIOSparkMax(),
                         new ElevatorEncoderIOThroughbore()); //TODO
         clawSubsystem =
                 new ClawSubsystem(new EndEffectorIOSparkMax());
         groundIntakeSubsystem =
-                new IntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace placeholder
+                new IntakeSubsystem(new IntakeIOSparkMax()); //TODO replace placeholder
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         armSubsystem =
@@ -181,7 +184,7 @@ public RobotContainer() {
         clawSubsystem =
                 new ClawSubsystem(new EndEffectorIO() {});
         groundIntakeSubsystem =
-                new IntakeSubsystem(new IntakeIOSparkMax(13, 12)); //TODO replace with real intake
+                new IntakeSubsystem(new IntakeIO() {});
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         armSubsystem =
@@ -224,16 +227,16 @@ void loadCommands() {
     operatorInput.resetElevatorEncoder.onTrue(new ResetElevatorEncoderCommand(elevatorSubsystem));
     operatorInput.manualPivotCommand.whileTrue(new IntakeSetPivotCommand(groundIntakeSubsystem, operatorController::getRightY));
     operatorInput.manualElevator.whileTrue(new ManualElevatorCommand(elevatorSubsystem, operatorController::getLeftY));
-
-
+    operatorInput.rollersIn.whileTrue(new IntakeSetRollersCommand(groundIntakeSubsystem, false));
+    operatorInput.rollersOut.whileTrue(new IntakeSetRollersCommand(groundIntakeSubsystem, true));
     // operatorInput.openClaw.onTrue(new OpenClawCommand(clawSubsystem));
     // operatorInput.closeClaw.onTrue(new CloseClawCommand(clawSubsystem));
 
     operatorInput.movementDesired.whileTrue(
             new BaseDriveCommand.basedrivecommand(
                 drive,
-                () -> -driveController.getLeftY(),
-                () -> -driveController.getLeftX(),
+                () -> -driveController.getLeftY() * -0.5,
+                () -> -driveController.getLeftX() * -0.5,
                 () -> -driveController.getRightX()));
   }
 

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorSetPointCommand.java

@@ -22,6 +22,7 @@ public ElevatorSetPointCommand(ElevatorSubsystem elevator, double targetHeight)
     @Override
     public void initialize(){
         elevator.elevatorPID.setGoal(targetHeight);
+        elevator.elevatorPID.reset(elevator.getLoadHeight());
         Logger.recordOutput("ElevatorSubsystem/target_height", targetHeight);
 
     }

src/main/java/frc/robot/commands/ElevatorCommands/ManualElevatorCommand.java

@@ -26,14 +26,14 @@ public void execute() {
         double calculatedVolts = elevator.elevatorFF.calculate(manualVelocity);
         Logger.recordOutput("ElevatorSubsystem/target_voltage", calculatedVolts);
         this.elevator.setElevatorVoltage(calculatedVolts);
-        if (elevator.getLoadHeight() >= ElevatorConstants.maxHeight) {
-            // We are going up and top limit is tripped so stop
-            elevator.setElevatorVoltage(kG);
-        }
-        if (elevator.getLoadHeight() <= ElevatorConstants.minHeight) {
-            // We are going down and bottom limit is tripped so stop
-            elevator.setElevatorVoltage(kG);
-        }
+//        if (elevator.getLoadHeight() >= ElevatorConstants.maxHeight) {
+//            // We are going up and top limit is tripped so stop
+//            elevator.setElevatorVoltage(kG);
+//        }
+//        if (elevator.getLoadHeight() <= ElevatorConstants.minHeight) {
+//            // We are going down and bottom limit is tripped so stop
+//            elevator.setElevatorVoltage(kG);
+//        }
     }
 
     @Override

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

@@ -14,10 +14,10 @@ public class ElevatorConstants {
     public static DCMotor elevator2Gearbox = DCMotor.getNEO(1);
     public static final int encoderA = 0;
     public static final int encoderB = 1;
-    public static final boolean isEncoderReversed = false;
+    public static final boolean isEncoderReversed = true;
 
     public static double gearingRatio = 60.0;
-    public static final double pulleyRatio = 1.0;
+    public static final double encoderReduction = 24.8977;
 
     public static double elevatorSparkEncoderPositionFactor = 2 * Math.PI / gearingRatio;
     public static double elevatorSparkEncoderVelocityFactor = 2 * Math.PI / 60 / gearingRatio;
@@ -34,8 +34,8 @@ public class ElevatorConstants {
     public static double kISim = 0;
     public static double kDSim = 0;
 
-    public static double kS = 0;
-    public static double kV = 1;
+    public static double kS = 2;
+    public static double kV = 11;
     public static double kG = 0;
     public static double kA = 0;
     public static double kP = 0;
@@ -48,17 +48,15 @@ public class ElevatorConstants {
     public static double L3 = 1.04775;
     public static double L4 = 1.7018;
     public static double carriageMass = 20.0;
-    public static double maxHeight = Units.inchesToMeters(67);
+    public static double maxHeight = Units.inchesToMeters(50);
     public static double minHeight = 0;
     public static double maxVelocity = 1;
     public static double maxAcceleration = 1;
 
     public static final int elevatorCanID = 10;
-    public static final int elevatorSpark1CAN = 15;
-    public static final int elevatorSpark2CAN = 16;
-    public static final boolean elevatorSpark1Inverted = false;
-    public static final boolean elevatorSpark2Inverted = true;
-
-
+    public static final int elevatorSpark1CAN = 14;
+    public static final int elevatorSpark2CAN = 15;
+    public static final boolean elevatorSpark1Inverted = true;
+    public static final boolean elevatorSpark2Inverted = false;
 
 }

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

@@ -12,10 +12,16 @@ public class IntakeConstants {
     public static double kP = 0.0;
     public static double kI = 0.0;
     public static double kD = 0.0;
-    public static double rollerVoltsTarget = 1.0;
+    // magnitude of the voltage to run rollers at
+    public static double rollerVoltsTarget = 5.0;
     public static final boolean rollerInverted = false;
-    public static double intakeVoltageFactor = 1;
-    public static int intakeMotorCurrentLimit = 20;
+
+    // scaling factor for manual control of algae intake
+
+    public static double intakeVoltageFactor = 8;
+    public static int intakeMotorCurrentLimit = 40;
     public static double pivotSparkEncoderPositionFactor = 2 * Math.PI / gearing;
     public static double pivotSparkEncoderVelocityFactor= 2 * Math.PI / 60 / gearing;
+    public static int pivotID = 12;
+    public static int rollerID = 13;
 }

src/main/java/frc/robot/subsystems/ElevatorSubsystem/ElevatorEncoderIOThroughbore.java

@@ -23,11 +23,12 @@ public void updateInputs(ElevatorEncoderIO.ElevatorEncoderIOInputs inputs) {
         inputs.encoderVelocityRots = encoder.getDistance() / (Timer.getFPGATimestamp() - lastTime);
         lastTime = Timer.getFPGATimestamp();
 
-        inputs.encoderVelocityRads = Units.rotationsToRadians(inputs.encoderVelocityRads);
-        inputs.encoderPositionRads = Units.rotationsToRadians(inputs.encoderPositionRads);
+        inputs.encoderVelocityRads = Units.rotationsToRadians(inputs.encoderVelocityRots);
+        inputs.encoderPositionRads = Units.rotationsToRadians(inputs.encoderPositionRots);
 
         // 1:1 with the shaft
-        inputs.unfilteredLoadHeight = inputs.encoderPositionRads * 2 * Math.PI / ElevatorConstants.pulleyRatio;
+        inputs.unfilteredLoadHeight = inputs.encoderPositionRads / ElevatorConstants.encoderReduction;
+
         inputs.loadHeight = elevatorFilter.calculate(inputs.unfilteredLoadHeight);
     }
 

src/main/java/frc/robot/subsystems/IntakeSubsystem/IntakeIOSparkMax.java

@@ -1,11 +1,13 @@
 package frc.robot.subsystems.IntakeSubsystem;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase;
 import com.revrobotics.spark.SparkLowLevel;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import frc.robot.constants.IntakeConstants;
+import frc.robot.util.SparkUtil;
 
 import static frc.robot.constants.DriveConstants.odometryFrequency;
 
@@ -14,9 +16,9 @@ public class IntakeIOSparkMax implements IntakeIO {
     private final SparkMax rollers;
     private final RelativeEncoder pivotEncoder;
 
-    public IntakeIOSparkMax(int pivotID, int rollersID) {
-        this.pivot = new SparkMax(pivotID, SparkLowLevel.MotorType.kBrushless);
-        this.rollers = new SparkMax(rollersID, SparkLowLevel.MotorType.kBrushless);
+    public IntakeIOSparkMax() {
+        this.pivot = new SparkMax(IntakeConstants.pivotID, SparkLowLevel.MotorType.kBrushless);
+        this.rollers = new SparkMax(IntakeConstants.rollerID, SparkLowLevel.MotorType.kBrushless);
 
         this.pivotEncoder = pivot.getEncoder();
         var intakeConfig = new SparkMaxConfig();
@@ -39,6 +41,10 @@ public IntakeIOSparkMax(int pivotID, int rollersID) {
                 .appliedOutputPeriodMs(20)
                 .busVoltagePeriodMs(20)
                 .outputCurrentPeriodMs(20);
+
+        SparkUtil.tryUntilOk(pivot, 5, () -> pivot.configure(
+                intakeConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters));
+
     }
 
     @Override
@@ -53,8 +59,8 @@ public void setRollersVoltage(double volts) {
 
     @Override
     public void updateInputs(IntakeIOInputs inputs) {
-        inputs.rollersVoltage = rollers.getBusVoltage();
-        inputs.pivotVoltage = pivot.getBusVoltage();
+        inputs.rollersVoltage = rollers.getAppliedOutput();
+        inputs.pivotVoltage = pivot.getAppliedOutput();
         inputs.pivotPosition = pivotEncoder.getPosition();
         inputs.pivotVelocity = pivotEncoder.getVelocity();
     }