more elevator and updated drive constants

firebears-frc · Feb 4, 2025 · 2b1f571 · 2b1f571
1 parent 1ea9170
commit 2b1f571
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,7 +17,6 @@
 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.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -35,21 +35,23 @@ public class DriveConstants {
       };
 
   // Zeroed rotation values for each module, see setup instructions
-  public static final Rotation2d frontLeftZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d frontRightZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0);
-  public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0);
+  public static final Rotation2d frontLeftZeroRotation = Rotation2d.fromDegrees(-90.0);
+  public static final Rotation2d frontRightZeroRotation = Rotation2d.fromDegrees(0.0);
+  public static final Rotation2d backLeftZeroRotation = Rotation2d.fromDegrees(180.0);
+  public static final Rotation2d backRightZeroRotation = Rotation2d.fromDegrees(90.0);
 
   // Device CAN IDs
   public static final int gyroCanId = 9;
 
   public static final int frontLeftDriveCanId = 1;
   public static final int backLeftDriveCanId = 5;
+
   public static final int frontRightDriveCanId = 3;
   public static final int backRightDriveCanId = 7;
 
   public static final int frontLeftTurnCanId = 2;
   public static final int backLeftTurnCanId = 6;
+
   public static final int frontRightTurnCanId = 4;
   public static final int backRightTurnCanId = 8;
 

diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -2,50 +2,78 @@
 
 import static frc.robot.util.SparkUtil.*;
 
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
 
 public class Elevator extends SubsystemBase {
-  private final SparkMax highElevatorMotor = new SparkMax(10, MotorType.kBrushless);
-  private final SparkMax lowElevatorMotor = new SparkMax(11, MotorType.kBrushless);
+  private final SparkMax leftElevatorMotor = new SparkMax(10, MotorType.kBrushless);
+  private SparkClosedLoopController leftClosedLoopController;
+  private RelativeEncoder elevatorEncoder = leftElevatorMotor.getEncoder();
+
+  private final SparkMax rightElevatorMotor = new SparkMax(11, MotorType.kBrushless);
+  private SparkClosedLoopController rightClosedLoopController;
+
+  @AutoLogOutput(key = "elevator/setPoint")
+  private Rotation2d elevatorSetpoint = new Rotation2d();
 
   public Elevator() {
-    var highSparkMaxConfig = new SparkMaxConfig();
+    var leftSparkMaxConfig = new SparkMaxConfig();
+    leftClosedLoopController = leftElevatorMotor.getClosedLoopController();
 
-    highSparkMaxConfig
+    leftSparkMaxConfig
         .idleMode(IdleMode.kBrake)
         .smartCurrentLimit(40, 40)
-        .secondaryCurrentLimit(50)
-        .inverted(sparkStickyFault);
+        .secondaryCurrentLimit(50);
 
-    highSparkMaxConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(0, 0, 0);
+    leftSparkMaxConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(0, 0, 0);
 
     tryUntilOk(
-        highElevatorMotor,
+        leftElevatorMotor,
         5,
         () ->
-            highElevatorMotor.configure(
-                highSparkMaxConfig,
+            leftElevatorMotor.configure(
+                leftSparkMaxConfig,
                 ResetMode.kResetSafeParameters,
                 PersistMode.kPersistParameters));
 
-    var lowSparkMaxConfig = new SparkMaxConfig();
-
-    lowSparkMaxConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(40, 40).secondaryCurrentLimit(50);
+    var rightSparkMaxConfig = new SparkMaxConfig();
+    rightClosedLoopController = rightElevatorMotor.getClosedLoopController();
 
-    lowSparkMaxConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(0, 0, 0);
+    rightSparkMaxConfig.follow(10, true);
 
     tryUntilOk(
-        lowElevatorMotor,
+        rightElevatorMotor,
         5,
         () ->
-            lowElevatorMotor.configure(
-                lowSparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+            rightElevatorMotor.configure(
+                rightSparkMaxConfig,
+                ResetMode.kResetSafeParameters,
+                PersistMode.kPersistParameters));
   }
+
+  public void setShoulderSetpoint(Rotation2d setpoint) { // change values later
+    if (setpoint.getDegrees() < -5) {
+      setpoint = Rotation2d.fromDegrees(-5);
+    } else if (setpoint.getDegrees() > 100) {
+      setpoint = Rotation2d.fromDegrees(100);
+    }
+    elevatorSetpoint = setpoint;
+  }
+
+  @AutoLogOutput(key = "elevator/rotations")
+  public Rotation2d getShoulderAngle() {
+    return Rotation2d.fromRotations(elevatorEncoder.getPosition());
+  }
+
+  public void periodic() {}
 }