tested all subsystems

hammerhead226 · Feb 24, 2024 · 05f18f2 · 05f18f2
1 parent f602db1
commit 05f18f2
Show file tree

Hide file tree

Showing 14 changed files with 188 additions and 140 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 90;
-  public static final String GIT_SHA = "3b8e10f3940dede6b9d8654e9943fa7abb0e6d0d";
-  public static final String GIT_DATE = "2024-02-20 00:54:38 EST";
+  public static final int GIT_REVISION = 93;
+  public static final String GIT_SHA = "f602db19c27b93de193ace204f2b8a46a187c8db";
+  public static final String GIT_DATE = "2024-02-23 19:03:52 EST";
   public static final String GIT_BRANCH = "main";
-  public static final String BUILD_DATE = "2024-02-22 20:21:31 EST";
-  public static final long BUILD_UNIX_TIME = 1708651291962L;
+  public static final String BUILD_DATE = "2024-02-23 21:51:21 EST";
+  public static final long BUILD_UNIX_TIME = 1708743081578L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -61,10 +61,10 @@ public static class IntakeConstants {
   }
 
   public static final class ShooterConstants {
-    public static final double FEEDER_CURRENT_LIMIT = 0;
+    public static final double FEEDER_CURRENT_LIMIT = 40;
     public static final boolean FEEDER_CURRENT_LIMIT_ENABLED = true;
 
-    public static final double FLYWHEEL_CURRENT_LIMIT = 0;
+    public static final double FLYWHEEL_CURRENT_LIMIT = 40;
     public static final boolean FLYWHEEL_CURRENT_LIMIT_ENABLED = true;
 
     public static final double FLYWHEEL_THRESHOLD = 0;
@@ -86,6 +86,9 @@ public static class ElevatorConstants {
     public static final double PIVOT_THRESHOLD = 0;
     public static final double EXTENDER_THRESHOLD = 0;
 
+    public static final double PIVOT_RATIO = 100.0;
+    public static final double EXTENDER_RATIO = 0;
+
     public static final double[] PIVOT_PID = {0, 0, 0};
     public static final double[] EXTENDER_PID = {0, 0, 0};
   }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -14,15 +14,12 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.Elevator.Elevator;
 import frc.robot.subsystems.Elevator.ElevatorExtenderIO;
@@ -79,17 +76,19 @@ public RobotContainer() {
                 new ModuleIOTalonFX(2),
                 new ModuleIOTalonFX(3));
         intake = new Intake(new IntakeRollerIOSparkFlex(RobotMap.IntakeIDs.ROLLERS));
-        // shooter =
-        //     new Shooter(
-        //         new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-        //         new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-        //         new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
-        // elevator =
-        //     new Elevator(
-        //         new ElevatorPivotIOTalonFX(
-        //             RobotMap.ElevatorIDs.PIVOT_ONE, RobotMap.ElevatorIDs.PIVOT_TWO),
-        //         new ElevatorExtenderIOTalonFX(
-        //             RobotMap.ElevatorIDs.EXTENDER_ONE, RobotMap.ElevatorIDs.EXTENDER_TWO));
+        shooter =
+            new Shooter(
+                new FlywheelIOTalonFX(
+                    RobotMap.ShooterIDs.FLYWHEEL_ONE, RobotMap.ShooterIDs.FLYWHEEL_TWO),
+                new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
+        elevator =
+            new Elevator(
+                new ElevatorPivotIOTalonFX(
+                    RobotMap.ElevatorIDs.PIVOT_ONE,
+                    RobotMap.ElevatorIDs.PIVOT_TWO,
+                    RobotMap.ElevatorIDs.GYRO),
+                new ElevatorExtenderIOTalonFX(
+                    RobotMap.ElevatorIDs.EXTENDER_ONE, RobotMap.ElevatorIDs.EXTENDER_TWO));
         // led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL));
         break;
       case REPLAY:
@@ -101,7 +100,7 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
+        shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim());
         elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
         led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL));
         break;
@@ -115,7 +114,7 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         intake = new Intake(new IntakeRollerIOSim());
-        shooter = new Shooter(new FlywheelIOSim(), new FlywheelIOSim(), new FeederIOSim());
+        shooter = new Shooter(new FlywheelIOSim(), new FeederIOSim());
         elevator = new Elevator(new ElevatorPivotIOSim(), new ElevatorExtenderIOSim());
         led = new LED(new LED_IOSpark(RobotMap.LEDIDs.CHANNEL));
         break;
@@ -132,13 +131,10 @@ public RobotContainer() {
                 new ModuleIO() {});
         shooter =
             new Shooter(
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
-                new FlywheelIOTalonFX(RobotMap.ShooterIDs.FLYWHEEL_ONE),
+                new FlywheelIOTalonFX(
+                    RobotMap.ShooterIDs.FLYWHEEL_ONE, RobotMap.ShooterIDs.FLYWHEEL_TWO),
                 new FeederIOTalonFX(RobotMap.ShooterIDs.FEEDER));
-        elevator =
-            new Elevator(
-                new ElevatorPivotIO() {},
-                new ElevatorExtenderIO() {});
+        elevator = new Elevator(new ElevatorPivotIO() {}, new ElevatorExtenderIO() {});
         led = new LED(new LED_IO() {});
         break;
     }
@@ -148,11 +144,9 @@ public RobotContainer() {
 
     // Intake SysID Routines
     autoChooser.addDefaultOption(
-        "Intake SysID (Dynamic Forward)",
-        intake.sysIdDynamic(SysIdRoutine.Direction.kForward));
+        "Intake SysID (Dynamic Forward)", intake.sysIdDynamic(SysIdRoutine.Direction.kForward));
     autoChooser.addDefaultOption(
-        "Intake SysID (Dynamic Reverse)",
-        intake.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+        "Intake SysID (Dynamic Reverse)", intake.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     autoChooser.addDefaultOption(
         "Intake SysID (Quasistatic Forward)",
         intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
@@ -175,18 +169,32 @@ private void configureButtonBindings() {
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-    controller
-        .b()
-        .onTrue(
-            Commands.runOnce(
-                    () ->
-                        drive.setPose(
-                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-                    drive)
-                .ignoringDisable(true));
-
-    controller.a().whileTrue(intake.sysIdQuasistatic(Direction.kForward));
+    // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+    // controller
+    //     .b()
+    //     .onTrue(
+    //         Commands.runOnce(
+    //                 () ->
+    //                     drive.setPose(
+    //                         new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+    //                 drive)
+    //             .ignoringDisable(true));
+
+    // controller.a().whileTrue(intake.sysIdQuasistatic(Direction.kForward));
+    // controller.a().onTrue(new SetPivotTarget(20, elevator));
+    // controller.a().onFalse(new InstantCommand(elevator::pivotStop, elevator));
+
+    // controller.a().onTrue(new SetExtenderTarget(0, elevator));
+    // controller.a().onFalse(new InstantCommand(elevator::elevatorStop, elevator));
+
+    controller.a().onTrue(new InstantCommand(() -> shooter.setShooterVelocitys(300, 300), shooter));
+    controller.a().onFalse(new InstantCommand(shooter::stopShooterMotors, shooter));
+
+    controller.b().onTrue(new InstantCommand(() -> shooter.runFeeders(1500), shooter));
+    controller.b().onFalse(new InstantCommand(shooter::stopFeeders, shooter));
+
+    // controller.a().onTrue(new SetExtenderTarget(0, elevator));
+    // controller.a().onFalse(new InstantCommand(elevator::elevatorStop, elevator));
 
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is
     // pressed,

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
@@ -6,17 +6,17 @@ public static class IntakeIDs {
   }
 
   public static class ShooterIDs {
-    public static int FLYWHEEL_ONE = 0; // TODO:: 11
-    public static int FLYWHEEL_TWO = 0; // TODO:: 12
-    public static int FEEDER = 0; // TODO:: 13
+    public static int FLYWHEEL_ONE = 11; // TODO:: 11
+    public static int FLYWHEEL_TWO = 12; // TODO:: 12
+    public static int FEEDER = 13; // TODO:: 13
   }
 
   public static class ElevatorIDs {
     public static final int GYRO = 10;
-    public static final int PIVOT_ONE = 0; // TODO:: 14
-    public static final int PIVOT_TWO = 0; // TODO:: 15
-    public static final int EXTENDER_ONE = 0; // TODO:: 8
-    public static final int EXTENDER_TWO = 0; // TODO:: 9 
+    public static final int PIVOT_ONE = 14; // TODO:: 14
+    public static final int PIVOT_TWO = 15; // TODO:: 15
+    public static final int EXTENDER_ONE = 8; // TODO:: 8
+    public static final int EXTENDER_TWO = 9; // TODO:: 9
   }
 
   public static class LEDIDs {

diff --git a/src/main/java/frc/robot/commands/SetExtenderTarget.java b/src/main/java/frc/robot/commands/SetExtenderTarget.java
@@ -5,13 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.RobotMap;
 import frc.robot.subsystems.Elevator.Elevator;
-import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim;
-import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX;
-import frc.robot.subsystems.Elevator.ElevatorPivotIOSim;
-import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX;
 
 public class SetExtenderTarget extends Command {
   /** Creates a new ExtendElevator. */
@@ -21,13 +15,11 @@ public class SetExtenderTarget extends Command {
 
   public SetExtenderTarget(double setPoint, Elevator elevator) {
     // Use addRequirements() here to declare subsystem dependencies.
-    //addRequirements(elevator);
+    // addRequirements(elevator);
     setPoint = this.setPoint;
     this.elevator = elevator;
-
   }
 
-
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
@@ -49,4 +41,4 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     return elevator.extenderAtSetpoint();
   }
-}
+}
diff --git a/src/main/java/frc/robot/commands/SetPivotTarget.java b/src/main/java/frc/robot/commands/SetPivotTarget.java
@@ -1,21 +1,17 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.RobotMap;
 import frc.robot.subsystems.Elevator.Elevator;
-import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim;
-import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX;
-import frc.robot.subsystems.Elevator.ElevatorPivotIOSim;
-import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX;
 
 public class SetPivotTarget extends Command {
   /** Creates a new SetPivotTarget. */
   private final Elevator elevator;
+
   private double setPoint;
+
   public SetPivotTarget(double setPoint, Elevator elevator) {
     setPoint = this.setPoint;
-     this.elevator = elevator;
+    this.elevator = elevator;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -35,4 +31,4 @@ public void end(boolean interrupted) {}
   public boolean isFinished() {
     return elevator.pivotAtSetpoint();
   }
-}
+}
diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
@@ -20,6 +20,9 @@ public class Elevator extends SubsystemBase {
   private static final LoggedTunableNumber extenderkP =
       new LoggedTunableNumber("elevatorExtenderkP");
 
+  TrapezoidProfile pivotProfile;
+  TrapezoidProfile extenderProfile;
+
   private final TrapezoidProfile.Constraints pivotConstraints =
       new TrapezoidProfile.Constraints(Math.PI / 4, Math.PI / 3);
   private TrapezoidProfile.State pivotGoal = new TrapezoidProfile.State();
@@ -39,10 +42,15 @@ public Elevator(ElevatorPivotIO pivot, ElevatorExtenderIO extender) {
 
     switch (Constants.currentMode) {
       case REAL:
-        elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4);
-        pivotFFModel = new ArmFeedforward(0, 0.4, 0.7);
-        pivotkP.initDefault(Constants.ElevatorConstants.PIVOT_PID[0]);
-        extenderkP.initDefault(Constants.ElevatorConstants.EXTENDER_PID[0]);
+        // elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4);
+        // pivotFFModel = new ArmFeedforward(0, 0.4, 0.7);
+        // pivotkP.initDefault(Constants.ElevatorConstants.PIVOT_PID[0]);
+        // extenderkP.initDefault(Constants.ElevatorConstants.EXTENDER_PID[0]);
+
+        elevatorFFModel = new ElevatorFeedforward(0, 0, 0.05);
+        pivotFFModel = new ArmFeedforward(0, 0, 0);
+        pivotkP.initDefault(0);
+        extenderkP.initDefault(0.04);
         break;
       case REPLAY:
         elevatorFFModel = new ElevatorFeedforward(0.02, 0.05, 1.4);
@@ -63,6 +71,15 @@ public Elevator(ElevatorPivotIO pivot, ElevatorExtenderIO extender) {
         extenderkP.initDefault(15);
         break;
     }
+    setPivotGoal(30);
+    pivotProfile = new TrapezoidProfile(pivotConstraints);
+
+    setExtenderGoal(-3);
+    extenderProfile = new TrapezoidProfile(extenderConstraints);
+    extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal);
+
+    // TODO:: set up default extender pose
+    pivotCurrent = pivotProfile.calculate(0, pivotCurrent, pivotGoal);
     pivotkP.initDefault(0);
     extenderkP.initDefault(15);
 
@@ -128,9 +145,6 @@ public void periodic() {
     pivot.updateInputs(pInputs);
     extender.updateInputs(eInputs);
 
-    TrapezoidProfile pivotProfile = new TrapezoidProfile(pivotConstraints);
-    TrapezoidProfile extenderProfile = new TrapezoidProfile(extenderConstraints);
-
     extenderCurrent =
         extenderProfile.calculate(Constants.LOOP_PERIOD_SECS, extenderCurrent, extenderGoal);
     pivotCurrent = pivotProfile.calculate(Constants.LOOP_PERIOD_SECS, pivotCurrent, pivotGoal);

diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java b/src/main/java/frc/robot/subsystems/Elevator/ElevatorExtenderIOTalonFX.java
@@ -11,6 +11,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants;
+import frc.robot.util.Conversions;
 
 public class ElevatorExtenderIOTalonFX implements ElevatorExtenderIO {
   private final TalonFX leader;
@@ -27,17 +28,17 @@ public ElevatorExtenderIOTalonFX(int lead, int follow) {
     config.CurrentLimits.StatorCurrentLimit = Constants.ElevatorConstants.EXTENDER_CURRENT_LIMIT;
     config.CurrentLimits.StatorCurrentLimitEnable =
         Constants.ElevatorConstants.EXTENDER_CURRENT_LIMIT_ENABLED;
-    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
 
-    leader = new TalonFX(lead);
-    follower = new TalonFX(follow);
+    leader = new TalonFX(lead, Constants.CANBUS);
+    follower = new TalonFX(follow, Constants.CANBUS);
 
     leader.getConfigurator().apply(config);
 
     positionSetpoint = Constants.ElevatorConstants.EXTENDER_RETRACT;
 
-    follower.setControl(new Follower(lead, false));
+    follower.setControl(new Follower(lead, true));
 
     elevatorPosition = leader.getPosition();
     elevatorVelocity = leader.getVelocity();
@@ -51,7 +52,8 @@ public ElevatorExtenderIOTalonFX(int lead, int follow) {
   @Override
   public void updateInputs(ElevatorExtenderIOInputs inputs) {
     BaseStatusSignal.refreshAll(elevatorPosition, elevatorVelocity, appliedVolts, currentAmps);
-    inputs.elevatorPosition = Units.rotationsToDegrees(elevatorPosition.getValueAsDouble());
+    inputs.elevatorPosition =
+        Conversions.motorRotToInches(elevatorPosition.getValueAsDouble(), 5.97, 15);
     inputs.elevatorVelocity =
         Units.rotationsPerMinuteToRadiansPerSecond(elevatorVelocity.getValueAsDouble());
     inputs.appliedVolts = appliedVolts.getValueAsDouble();
@@ -61,7 +63,16 @@ public void updateInputs(ElevatorExtenderIOInputs inputs) {
   @Override
   public void setPositionSetpoint(double position, double ffVolts) {
     this.positionSetpoint = position;
-    leader.setControl(new PositionVoltage(position, 0, false, ffVolts, 0, false, false, false));
+    leader.setControl(
+        new PositionVoltage(
+            Conversions.inchesToMotorRot(position, 5.97, 15),
+            0,
+            false,
+            ffVolts,
+            0,
+            false,
+            false,
+            false));
   }
 
   @Override