a lot

Spikes-2212 · Spikes-2212 · commit 39ce67a88a8c · 2025-03-05T13:25:39.000+02:00
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -9,40 +9,39 @@
 import com.spikes2212.dashboard.RootNamespace;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.commands.*;
 import frc.robot.commands.autonomous.JustDrive;
 import frc.robot.commands.district2.District2RotateStorage;
-import frc.robot.subsystems.Drivetrain;
-import frc.robot.subsystems.Storage;
+import frc.robot.subsystems.*;
 import frc.robot.subsystems.district2.District2CoralJoint;
 
 public class Robot extends TimedRobot {
 
     RootNamespace namespace = new RootNamespace("robot");
     private Drivetrain drivetrain;
-//    private Elevator elevator;
+    private Elevator elevator;
     private Storage storage;
-//    private Gripper gripper;
+    private Gripper gripper;
+    private AlgaeJoint algaeJoint;
     private District2CoralJoint coralJoint;
     OI oi = new OI();
 
-//    private AlgaeJoint algaeJoint;
-
     @Override
     public void robotInit() {
         drivetrain = Drivetrain.getInstance();
         namespace.putNumber("left x", oi::getLeftX);
         namespace.putNumber("left y", oi::getLeftY);
         namespace.putNumber("right x", oi::getRightX);
         namespace.putNumber("right y", oi::getRightY);
+        algaeJoint = AlgaeJoint.getInstance();
         getInstances();
     }
 
     @Override
     public void robotPeriodic() {
         CommandScheduler.getInstance().run();
         coralJoint.calibrateEncoderPosition();
+//        elevator.calibratePosition();
         namespace.update();
     }
 
@@ -74,6 +73,7 @@ public void autonomousPeriodic() {
 
     @Override
     public void teleopInit() {
+        coralJoint.coast();
         drivetrain.resetRelativeEncoders();
         drivetrain.setNeutralMode(NeutralModeValue.Coast);
         drivetrain.setDefaultCommand(new Drive(drivetrain, () -> oi.getLeftY() * 4, () -> oi.getLeftX() * 4, () -> oi.getRightX() * 6,
@@ -127,10 +127,10 @@ private void registerNamedCommands() {
     }
 
     private void getInstances() {
-//        elevator = Elevator.getInstance();
+        elevator = Elevator.getInstance();
         storage = Storage.getInstance();
-//        gripper = Gripper.getInstance();
+        gripper = Gripper.getInstance();
         coralJoint = District2CoralJoint.getInstance();
-//        algaeJoint = AlgaeJoint.getInstance();
+        algaeJoint = AlgaeJoint.getInstance();
     }
 }
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
@@ -19,8 +19,8 @@ public interface CAN {
         int BACK_LEFT_ABSOLUTE_ENCODER = 9;
         int BACK_RIGHT_ABSOLUTE_ENCODER = 12;
 
-        int ELEVATOR_MASTER_SPARK = -1;
-        int ELEVATOR_SLAVE_SPARK = -1;
+        int ELEVATOR_MASTER_SPARK = 18;
+        int ELEVATOR_SLAVE_SPARK = 19;
         int ALGAE_JOINT_SPARK = 13;
         int CORAL_JOINT_TALON = 16;
         int GRIPPER_VICTOR = 17;
@@ -29,13 +29,11 @@ public interface CAN {
 
     public interface DIO {
 
-        int ELEVATOR_TOP_LIMIT = -1;
-        int ELEVATOR_BOTTOM_LIMIT = -1;
-        int ALGAE_TOP_LIMIT = 0;
-        int CORAL_JOINT_TOP_LIMIT = -1;
-        int CORAL_JOINT_BOTTOM_LIMIT = 8;
-        int GRIPPER_LIMIT = 5;
-        int STORAGE_INFRARED = 3;
+        int ELEVATOR_TOP_LIMIT = 2;
+        int ELEVATOR_BOTTOM_LIMIT = 6;
+        int CORAL_JOINT_BOTTOM_LIMIT = 4;
+        int GRIPPER_LIMIT = 0;
+        int STORAGE_INFRARED = 9;
     }
 
     public interface PWM {
diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
@@ -26,7 +26,7 @@ public class Drive extends Command {
     private final SlewRateLimiter yLimiter;
     private final SlewRateLimiter rotationLimiter;
 
-    double time;
+    private double time;
 
     public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,
                  Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,
@@ -62,7 +62,6 @@ public void execute() {
         }
 
         drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID, Timer.getFPGATimestamp() - time);
-//        logger.log(Timer.getFPGATimestamp() - time);
         time = Timer.getFPGATimestamp();
     }
 
diff --git a/src/main/java/frc/robot/commands/IntakeCoral.java b/src/main/java/frc/robot/commands/IntakeCoral.java
@@ -30,7 +30,6 @@ public void initialize() {
 
     @Override
     public boolean isFinished() {
-//        return false;
         return super.isFinished() && Timer.getFPGATimestamp() - startTime >= TIME_TO_INTAKE.get();
     }
 
diff --git a/src/main/java/frc/robot/commands/MoveToHeight.java b/src/main/java/frc/robot/commands/MoveToHeight.java
@@ -1,7 +1,6 @@
 package frc.robot.commands;
 
-import com.spikes2212.command.genericsubsystem.commands.smartmotorcontrollergenericsubsystem.
-        MoveSmartMotorControllerGenericSubsystem;
+import com.spikes2212.command.genericsubsystem.commands.smartmotorcontrollergenericsubsystem.MoveSmartMotorControllerGenericSubsystem;
 import com.spikes2212.control.FeedForwardController;
 import com.spikes2212.control.FeedForwardSettings;
 import com.spikes2212.control.PIDSettings;
diff --git a/src/main/java/frc/robot/commands/PickAlgae.java b/src/main/java/frc/robot/commands/PickAlgae.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.AlgaeJoint;
-import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Gripper;
 
 public class PickAlgae extends SequentialCommandGroup {
diff --git a/src/main/java/frc/robot/commands/ReleaseAlgae.java b/src/main/java/frc/robot/commands/ReleaseAlgae.java
@@ -4,6 +4,7 @@
 import com.spikes2212.dashboard.RootNamespace;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.subsystems.Gripper;
+
 import java.util.function.Supplier;
 
 public class ReleaseAlgae extends MoveGenericSubsystem {
diff --git a/src/main/java/frc/robot/commands/Reset.java b/src/main/java/frc/robot/commands/Reset.java
@@ -1,11 +1,9 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.AlgaeJoint;
 import frc.robot.subsystems.CoralJoint;
 import frc.robot.subsystems.Elevator;
-import frc.robot.subsystems.Storage;
 
 public class Reset extends ParallelCommandGroup {
 
diff --git a/src/main/java/frc/robot/commands/RotateAlgaeJointToBottom.java b/src/main/java/frc/robot/commands/RotateAlgaeJointToBottom.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-import com.spikes2212.command.genericsubsystem.GenericSubsystem;
 import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
 import com.spikes2212.dashboard.RootNamespace;
 import edu.wpi.first.wpilibj.Timer;
diff --git a/src/main/java/frc/robot/commands/RotateAlgaeJointToTop.java b/src/main/java/frc/robot/commands/RotateAlgaeJointToTop.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-import com.spikes2212.command.genericsubsystem.GenericSubsystem;
 import com.spikes2212.command.genericsubsystem.commands.MoveGenericSubsystem;
 import com.spikes2212.dashboard.RootNamespace;
 import frc.robot.subsystems.AlgaeJoint;
diff --git a/src/main/java/frc/robot/commands/district2/District2PlaceOnReef.java b/src/main/java/frc/robot/commands/district2/District2PlaceOnReef.java
@@ -1,12 +1,7 @@
 package frc.robot.commands.district2;
 
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.commands.MoveToHeight;
 import frc.robot.commands.ReleaseCoral;
-import frc.robot.commands.RotateStorage;
-import frc.robot.subsystems.CoralJoint;
-import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Storage;
 import frc.robot.subsystems.district2.District2CoralJoint;
 
diff --git a/src/main/java/frc/robot/commands/district2/District2Reset.java b/src/main/java/frc/robot/commands/district2/District2Reset.java
@@ -1,13 +1,8 @@
 package frc.robot.commands.district2;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.MoveToHeight;
 import frc.robot.commands.RotateAlgaeJointToTop;
-import frc.robot.commands.RotateStorage;
 import frc.robot.subsystems.AlgaeJoint;
-import frc.robot.subsystems.CoralJoint;
-import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.district2.District2CoralJoint;
 
 public class District2Reset extends ParallelCommandGroup {
diff --git a/src/main/java/frc/robot/commands/district2/District2RotateStorage.java b/src/main/java/frc/robot/commands/district2/District2RotateStorage.java
@@ -15,24 +15,22 @@ public class District2RotateStorage extends MoveGenericSubsystemWithPID {
 
     private static final RootNamespace namespace = new RootNamespace("district 2 rotate storage");
     private static final PIDSettings intakePIDSettings = namespace.addPIDNamespace("intake rotate storage",
-            new PIDSettings(0.11, 0, 0.005, 0, 3, 0.5));
+            new PIDSettings(0.2, 0.07, 0.012, 30, 4, 0.5));
     private static final FeedForwardSettings intakeFeedForwardSettings = namespace.addFeedForwardNamespace("intake rotate storage",
-            new FeedForwardSettings(0, 0, 0, 0.082, FeedForwardController.ControlMode.ANGULAR_POSITION));
+            new FeedForwardSettings(0, 0, 0, 0.13, FeedForwardController.ControlMode.ANGULAR_POSITION));
     private static final PIDSettings outtakePIDSettings = namespace.addPIDNamespace("outtake rotate storage",
-            new PIDSettings(0.15, 0, 0.012, 0, 3, 0.5));
+            new PIDSettings(0.2, 0.002, 0.018, 4, 4, 0.5));
     private static final FeedForwardSettings outtakeFeedForwardSettings = namespace.addFeedForwardNamespace("outtake rotate storage",
-            new FeedForwardSettings(0, 0, 0, 0.12, FeedForwardController.ControlMode.ANGULAR_POSITION));
+            new FeedForwardSettings(0, 0, 0, 0.13, FeedForwardController.ControlMode.ANGULAR_POSITION));
 
     private final District2CoralJoint coralJoint;
-    private SpikesLogger logger = new SpikesLogger();
 
     public District2RotateStorage(District2CoralJoint coralJoint, Supplier<Double> setpoint) {
         super(coralJoint, () -> Math.toRadians(setpoint.get()), () -> Math.toRadians(coralJoint.getPosition()),
                 Storage.getInstance().hasCoral() ? outtakePIDSettings : intakePIDSettings,
                 Storage.getInstance().hasCoral() ? outtakeFeedForwardSettings : intakeFeedForwardSettings);
         pidSettings.setWaitTime(() -> 999.0);
         this.coralJoint = coralJoint;
-        logger.log("m" + pidSettings.getWaitTime());
     }
 
     public District2RotateStorage(District2CoralJoint coralJoint, District2CoralJoint.StoragePose pose) {
@@ -60,7 +58,6 @@ protected double calculatePIDAndFFValues() {
 
     @Override
     public boolean isFinished() {
-        logger.log(source.get() + ", " + setpoint.get());
         return !(coralJoint.canMove(coralJoint.getSpeed())) || super.isFinished();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/AlgaeJoint.java b/src/main/java/frc/robot/subsystems/AlgaeJoint.java
@@ -3,11 +3,11 @@
 import com.revrobotics.spark.SparkLowLevel;
 import com.revrobotics.spark.config.SparkBaseConfig;
 import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.RobotMap;
 import frc.robot.commands.RotateAlgaeJointToBottom;
 import frc.robot.commands.RotateAlgaeJointToTop;
 import frc.robot.util.SparkWrapper;
-import edu.wpi.first.wpilibj.DigitalInput;
-import frc.robot.RobotMap;
 
 public class AlgaeJoint extends SmartMotorControllerGenericSubsystem {
 
@@ -16,34 +16,26 @@ public class AlgaeJoint extends SmartMotorControllerGenericSubsystem {
     private static final String NAMESPACE_NAME = "storage";
 
     private final SparkWrapper spark;
-    private final DigitalInput topLimit;
 
     private static AlgaeJoint instance;
 
     public static AlgaeJoint getInstance() {
         if (instance == null) {
             instance = new AlgaeJoint(SparkWrapper.createSparkMax(RobotMap.CAN.ALGAE_JOINT_SPARK,
-                    SparkLowLevel.MotorType.kBrushless), new DigitalInput(RobotMap.DIO.ALGAE_TOP_LIMIT));
+                    SparkLowLevel.MotorType.kBrushless));
         }
         return instance;
     }
 
-    private AlgaeJoint(SparkWrapper spark, DigitalInput topLimit) {
+    private AlgaeJoint(SparkWrapper spark) {
         super(NAMESPACE_NAME, spark);
         this.spark = spark;
-        this.topLimit = topLimit;
         spark.setIdleMode(SparkBaseConfig.IdleMode.kBrake);
         configureDashboard();
     }
 
-    @Override
-    public boolean canMove(double speed) {
-        return !(speed > 0 && topLimit.get());
-    }
-
     @Override
     public void configureDashboard() {
-        namespace.putBoolean("top limit", topLimit::get);
         namespace.putCommand("rotate to top", new RotateAlgaeJointToTop(this));
         namespace.putCommand("rotate to bottom", new RotateAlgaeJointToBottom(this));
         namespace.putNumber("encoder", spark::getPosition);
diff --git a/src/main/java/frc/robot/subsystems/CoralJoint.java b/src/main/java/frc/robot/subsystems/CoralJoint.java
@@ -1,9 +1,6 @@
 package frc.robot.subsystems;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.revrobotics.spark.SparkLowLevel;
 import com.spikes2212.command.genericsubsystem.smartmotorcontrollersubsystem.SmartMotorControllerGenericSubsystem;
-import com.spikes2212.util.smartmotorcontrollers.SparkWrapper;
 import com.spikes2212.util.smartmotorcontrollers.TalonFXWrapper;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc.robot.RobotMap;
@@ -30,7 +27,6 @@ public enum StoragePose {
     private static final double DISTANCE_PER_PULSE = GEAR_RATIO * DEGREES_IN_ROTATIONS;
 
     private final TalonFXWrapper talonFX;
-    private final DigitalInput topLimit;
     private final DigitalInput bottomLimit;
 
     private static CoralJoint instance;
@@ -39,36 +35,31 @@ public static CoralJoint getInstance() {
         if (instance == null) {
             instance = new CoralJoint(NAMESPACE_NAME,
                     new TalonFXWrapper(RobotMap.CAN.CORAL_JOINT_TALON),
-                    new DigitalInput(RobotMap.DIO.CORAL_JOINT_TOP_LIMIT),
                     new DigitalInput(RobotMap.DIO.CORAL_JOINT_BOTTOM_LIMIT));
         }
         return instance;
     }
 
-    private CoralJoint(String namespaceName, TalonFXWrapper talonFX, DigitalInput topLimit, DigitalInput bottomLimit) {
+    private CoralJoint(String namespaceName, TalonFXWrapper talonFX, DigitalInput bottomLimit) {
         super(namespaceName, talonFX);
         this.talonFX = talonFX;
-        this.topLimit = topLimit;
         this.bottomLimit = bottomLimit;
         talonFX.setEncoderConversionFactor(DISTANCE_PER_PULSE);
         configureDashboard();
     }
 
     @Override
     public boolean canMove(double speed) {
-        return !((bottomLimit.get() && speed < 0) || (topLimit.get() && speed > 0));
+        return !(bottomLimit.get() && speed < 0);
     }
 
     public void calibrateEncoderPosition() {
-        if (topLimit.get()) {
-            talonFX.setPosition(StoragePose.RESTING.neededPitch);
-        } else if (bottomLimit.get()) {
+        if (bottomLimit.get()) {
             talonFX.setPosition(StoragePose.PLACEMENT.neededPitch);
         }
     }
 
     public void configureDashboard() {
-        namespace.putBoolean("top limit", topLimit::get);
         namespace.putBoolean("bottom limit", bottomLimit::get);
         namespace.putNumber("storage pose", talonFX::getPosition);
     }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -57,9 +57,9 @@ public class Drivetrain extends DashboardedSubsystem {
     private final SwerveDriveOdometry odometry;
     private final VisionService visionService;
     private final SysIdRoutine sysIdRoutine;
-    StructArrayPublisher<SwerveModuleState> currentStates = NetworkTableInstance.getDefault()
+    private final StructArrayPublisher<SwerveModuleState> currentStates = NetworkTableInstance.getDefault()
             .getStructArrayTopic("current states", SwerveModuleState.struct).publish();
-    StructArrayPublisher<SwerveModuleState> desiredStates = NetworkTableInstance.getDefault()
+    private final StructArrayPublisher<SwerveModuleState> desiredStates = NetworkTableInstance.getDefault()
             .getStructArrayTopic("desired states", SwerveModuleState.struct).publish();
 
     private SwerveModulePosition[] swerveModulePositions;
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
diff --git a/src/main/java/frc/robot/subsystems/Storage.java b/src/main/java/frc/robot/subsystems/Storage.java
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java b/src/main/java/frc/robot/subsystems/SwerveModuleHolder.java
diff --git a/src/main/java/frc/robot/subsystems/district2/District2CoralJoint.java b/src/main/java/frc/robot/subsystems/district2/District2CoralJoint.java

Original file line number	Diff line number	Diff line change
`@@ -26,7 +26,7 @@ public class Drive extends Command {`
`26`	`26`	`private final SlewRateLimiter yLimiter;`
`27`	`27`	`private final SlewRateLimiter rotationLimiter;`
`28`	`28`
`29`		`- double time;`
	`29`	`+ private double time;`
`30`	`30`
`31`	`31`	`public Drive(Drivetrain drivetrain, Supplier<Double> xSpeed, Supplier<Double> ySpeed,`
`32`	`32`	`Supplier<Double> rotationSpeed, boolean fieldRelative, boolean usePID,`
`@@ -62,7 +62,6 @@ public void execute() {`
`62`	`62`	`}`
`63`	`63`
`64`	`64`	`drivetrain.drive(xSpeed, ySpeed, rotationSpeed, fieldRelative, usePID, Timer.getFPGATimestamp() - time);`
`65`		`-// logger.log(Timer.getFPGATimestamp() - time);`
`66`	`65`	`time = Timer.getFPGATimestamp();`
`67`	`66`	`}`
`68`	`67`
Original file line number	Diff line number	Diff line change
`@@ -30,7 +30,6 @@ public void initialize() {`
`30`	`30`
`31`	`31`	`@Override`
`32`	`32`	`public boolean isFinished() {`
`33`		`-// return false;`
`34`	`33`	`return super.isFinished() && Timer.getFPGATimestamp() - startTime >= TIME_TO_INTAKE.get();`
`35`	`34`	`}`
`36`	`35`