Merge branch 'workingongettingready' of https://github.com/RegisJesui…

…tRobotics/Competition2024 into workingongettingready

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

@@ -24,16 +24,16 @@ public static class ScoringConstants {
 
   public static class IntakeConstants {
     public static final int INTAKE_VOLTAGE = 3;
-    public static final int INTAKE_MOTOR_ID = 15;
+    public static final int INTAKE_MOTOR_ID = 12;
 
     public static final int STALL_MOTOR_CURRENT = 40;
     public static final int FREE_MOTOR_CURRENT = 20;
   }
 
   public static class SlapdownConstants {
 
-    public static final int FEEDER_MOTOR_ID = 16;
-    public static final int ROTATION_MOTOR_ID = 17;
+    public static final int FEEDER_MOTOR_ID = 14;
+    public static final int ROTATION_MOTOR_ID = 13;
     public static final double ROTATION_GEAR_RATIO = 5.0 * 3.0 * 30.0 / 15.0;
 
     public static final int FEED_STALL_MOTOR_CURRENT = 20;
@@ -60,7 +60,7 @@ public static class ElevatorConstants {
     public static final int STALL_MOTOR_CURRENT = 45;
     public static final int FREE_MOTOR_CURRENT = 25;
 
-    public static final int ELEVATOR_MOTOR_ID = 14;
+    public static final int ELEVATOR_MOTOR_ID = 15;
 
     public static final int ELEVATOR_LIMIT_SWITCH = 2;
 
@@ -146,18 +146,18 @@ private SwerveConstants() {}
 
     public static final SwerveModuleConfiguration FRONT_LEFT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            1, 4, 9, false, true, -0.7455146908760071, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            1, 5, 17, false, true, -0.7455146908760071, false, SHARED_SWERVE_MODULE_CONFIGURATION);
     public static final SwerveModuleConfiguration FRONT_RIGHT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            2, 6, 10, false, true, 2.7857091108003242, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            2, 6, 18, false, true, 2.7857091108003242, false, SHARED_SWERVE_MODULE_CONFIGURATION);
 
     public static final SwerveModuleConfiguration BACK_LEFT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            3, 7, 11, false, true, 2.676796474860444, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            3, 7, 19, false, true, 2.676796474860444, false, SHARED_SWERVE_MODULE_CONFIGURATION);
 
     public static final SwerveModuleConfiguration BACK_RIGHT_MODULE_CONFIGURATION =
         new SwerveModuleConfiguration(
-            4, 8, 12, false, true, -2.4605051837685683, false, SHARED_SWERVE_MODULE_CONFIGURATION);
+            4, 8, 20, false, true, -2.4605051837685683, false, SHARED_SWERVE_MODULE_CONFIGURATION);
   }
 
   public static class AutoConstants {
@@ -197,7 +197,7 @@ public static class WristConstants {
     public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60));
     public static final Rotation2d WRIST_MIN = new Rotation2d(0);
 
-    public static final int WRIST_MOTOR_ID = 11;
+    public static final int WRIST_MOTOR_ID = 9;
     public static final int STALL_MOTOR_CURRENT = 45;
     public static final int FREE_MOTOR_CURRENT = 25;
 
@@ -212,7 +212,7 @@ public static class WristConstants {
   }
 
   public static class TransportConstants {
-    public static final int TRANSPORT_MOTOR_ID = 0;
+    public static final int TRANSPORT_MOTOR_ID = 11;
     public static final int STALL_MOTOR_CURRENT = 45;
     public static final int FREE_MOTOR_CURRENT = 25;
     public static final double TRANSPORT_VOLTAGE = 2.0;
@@ -222,7 +222,7 @@ public static class ShooterConstants {
     public static final int FREE_MOTOR_CURRENT = 25;
     public static final int STALL_MOTOR_CURRENT = 45;
 
-    public static final int SHOOTER_ID = 18;
+    public static final int SHOOTER_ID = 10;
 
     public static final double SHOOTING_RPM = 3000.0;
 

src/main/java/frc/robot/FieldConstants.java

@@ -35,8 +35,8 @@ public class FieldConstants {
   public static final class StagingLocations {
     public static double centerlineX = fieldLength / 2.0;
     // TODO: FIX THIS
-    public static final double stagingThreshold = 0;
-    public static final double ampThreshold = 0;
+    public static final double stagingThreshold = 5;
+    public static final double ampThreshold = 5;
 
     // need to update
     public static double centerlineFirstY = Units.inchesToMeters(29.638);

src/main/java/frc/robot/commands/elevator/AmpPlaceCommand.java

@@ -1,11 +1,10 @@
 package frc.robot.commands.elevator;
 
-import static frc.robot.Constants.ElevatorConstants.*;
+import static frc.robot.Constants.ScoringConstants.*;
 import static frc.robot.Constants.WristConstants.*;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.shooter.ShootAtAngleCommand;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.transport.TransportSubsystem;
@@ -27,8 +26,8 @@ public AmpPlaceCommand(
     this.wristSubsystem = wristSubsystem;
     this.transportSubsystem = transportSubsystem;
     addCommands(
-        new ElevatorToPositionCommand(elevatorSubsystem, ELEVATOR_AMP_POS),
-        new WristToPositionCommand(wristSubsystem, WRIST_AMP_POSITION),
+        elevatorSubsystem.setElevatorPositionCommand(AMP_ELEVATOR_HEIGHT),
+        wristSubsystem.setPosiitonCommand(WRIST_AMP_POSITION),
         new ShootAtAngleCommand(
             shooterSubsystem, transportSubsystem, wristSubsystem, WRIST_AMP_POSITION));
   }

src/main/java/frc/robot/commands/elevator/ElevatorWristParallelCommand.java

@@ -1,10 +1,8 @@
 package frc.robot.commands.elevator;
 
-import static frc.robot.Constants.ElevatorConstants.*;
 import static frc.robot.Constants.WristConstants.*;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
 
@@ -18,7 +16,7 @@ public ElevatorWristParallelCommand(
     this.wristSubsystem = wristSubsystem;
 
     addCommands(
-        new WristToPositionCommand(wristSubsystem, WRIST_MIN),
-        new ElevatorToPositionCommand(elevatorSubsystem, 0));
+        wristSubsystem.setPosiitonCommand(WRIST_MIN),
+        elevatorSubsystem.setElevatorPositionCommand(0));
   }
 }

src/main/java/frc/robot/commands/shooter/RunWristAndFlywheels.java

@@ -4,7 +4,6 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.wrist.WristToPositionCommand;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
 
@@ -19,7 +18,6 @@ public RunWristAndFlywheels(
     this.shooterSubsystem = shooterSubsystem;
 
     addCommands(
-        new WristToPositionCommand(wristSubsystem, shootingAngle),
-        shooterSubsystem.runRPMCommand(SHOOTING_RPM));
+        wristSubsystem.setPosiitonCommand(shootingAngle), shooterSubsystem.runFlyRPM(SHOOTING_RPM));
   }
 }

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

@@ -7,6 +7,7 @@
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -88,6 +89,12 @@ private void configMotors() {
     elevatorAlert.set(faultInitializing);
   }
 
+  public void atBottomLimit() {
+    if (bottomLimit.get()) {
+      elevatorEncoder.setPosition(0);
+    }
+  }
+
   public void setDesiredPosition(double desiredPosition) {
     // TODO: CLAMP THIS
     controller.setGoal(desiredPosition);
@@ -97,6 +104,10 @@ public boolean atGoal() {
     return controller.atGoal();
   }
 
+  public void setEncoderPosition(double position) {
+    elevatorEncoder.setPosition(position);
+  }
+
   public void setVoltage(double voltage) {
     elevatorMotor.setVoltage(voltage);
   }
@@ -106,7 +117,7 @@ public double getPosition() {
   }
 
   public void stopMove() {
-    setVoltage(0.0);
+    elevatorMotor.setVoltage(0);
   }
 
   // TODO: Fix
@@ -124,19 +135,27 @@ public Command runElevatorCommand(double voltage) {
 
   // TODO: Fix
   public Command setElevatorPositionCommand(double position) {
-    return this.run(() -> this.setDesiredPosition(position));
+    return this.run(
+        () -> {
+          controller.setGoal(position);
+          double feedbackOutput = controller.calculate(getPosition());
+          TrapezoidProfile.State currentSetpoint = controller.getSetpoint();
+
+          setVoltage(
+              feedbackOutput + feedforward.calculate(getPosition(), currentSetpoint.velocity));
+        });
   }
 
   @Override
   public void periodic() {
-    // TODO: Log that this is being reset, or maybe don't always reset?
-    if (bottomLimit.get()) {
-      elevatorEncoder.setPosition(0);
-    }
+    logValues();
   }
 
   private void logValues() {
-    bottomLimitEntry.append(bottomLimit.get());
     elevatorMotor.logValues();
+    bottomLimitEntry.append(bottomLimit.get());
+    if (FF_GAINS.hasChanged()) {
+      feedforward = FF_GAINS.createFeedforward();
+    }
   }
 }

src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java

@@ -143,7 +143,15 @@ private double getPosition() {
   }
 
   public Command setRotationGoalCommand(Rotation2d goal) {
-    return this.run(() -> this.setRotationGoal(goal)).until(rotationController::atGoal);
+    return this.run(
+        () -> {
+          this.setRotationGoal(goal);
+          double feedbackOutput = rotationController.calculate(getPosition());
+          TrapezoidProfile.State currentSetpoint = rotationController.getSetpoint();
+
+          setRotationVoltage(
+              feedbackOutput + rotationFF.calculate(getPosition(), currentSetpoint.velocity));
+        });
   }
 
   public Command setFeederVoltageCommand(double voltage) {