Merge branch 'George-ArmPrototype' of https://github.com/TitaniumTita…

…ns/2024Crescendo into George-ArmPrototype

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.peterabeles.gversion" version "1.10"
     id "org.sonarqube" version "4.4.1.3373"
 }

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

@@ -54,15 +54,15 @@ private DriveConstants() {
     // kP, kI, kD in order
     protected static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0};
     // kP, kI, kD in order
-    protected static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.0};
+    protected static final double[] TURN_FB_GAINS = new double[]{0.1, 0.0, 0.0};
 
     public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants(
             0,
             new int[]{0, 1, 2}, // drive, turn, encoder
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.457764), // offset
+            Units.rotationsToDegrees(-0.017578), // offset 0.457764
             true, // inversion
             ModuleConstants.GearRatios.L3
     );
@@ -84,8 +84,8 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(-0.017578),
-            false,
+            Units.rotationsToDegrees(0.457764),
+            true,
             ModuleConstants.GearRatios.L3
     );
 
@@ -96,7 +96,7 @@ private DriveConstants() {
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
             Units.rotationsToDegrees(-0.263916),
-            false,
+            true,
             ModuleConstants.GearRatios.L3
     );
   }

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

@@ -58,7 +58,9 @@ public class RobotContainer {
   private final LoggedDashboardChooser<Command> autoChooser;
 
   private final LoggedDashboardNumber wristPower;
+  private final LoggedDashboardNumber wristPosition;
   private final LoggedDashboardNumber armPower;
+  private final LoggedDashboardNumber armPosition;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -67,10 +69,10 @@ public RobotContainer() {
           // Real robot, instantiate hardware IO implementations
           m_driveSubsystem = new DriveSubsystem(
             new GyroIOPigeon1(12),
-              new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
+            new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
           m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
           m_armSubsystem = new ArmSubsystem(new ArmIOPrototype());
         }
@@ -83,7 +85,7 @@ public RobotContainer() {
                   new ModuleIO() {},
                   new ModuleIO() {});
           m_shooter = new ShooterSubsystem(new ShooterIO() {});
-          m_armSubsystem = new ArmSubsystem(new ArmIOPrototype());
+          m_armSubsystem = new ArmSubsystem(new ArmIOPrototype() {});
     }
     case SIM -> {
       // Sim robot, instantiate physics sim IO implementations
@@ -122,7 +124,9 @@ public RobotContainer() {
 
 
     armPower = new LoggedDashboardNumber("Arm Power", 0.0);
+    armPosition = new LoggedDashboardNumber("Arm Position", 0.0);
     wristPower = new LoggedDashboardNumber("Wrist Power", 0.0);
+    wristPosition = new LoggedDashboardNumber("Wrist Position", 0.0);
 
     // Set up feedforward characterization
     autoChooser.addOption(
@@ -146,29 +150,33 @@ private void configureButtonBindings() {
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(
                 m_driveSubsystem,
-            () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
+            () -> -controller.getLeftY(),
             () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(m_driveSubsystem::stopWithX, m_driveSubsystem));
-    controller
-        .b()
-        .onTrue(
-            Commands.runOnce(
-                    () ->
-                        m_driveSubsystem.setPose(
-                            new Pose2d(m_driveSubsystem.getPose().getTranslation(), new Rotation2d())),
-                            m_driveSubsystem)
-                .ignoringDisable(true));
+//    controller.x().onTrue(Commands.runOnce(m_driveSubsystem::stopWithX, m_driveSubsystem));
+//    controller
+//        .b()
+//        .onTrue(
+//            Commands.runOnce(
+//                    () ->
+//                        m_driveSubsystem.setPose(
+//                            new Pose2d(m_driveSubsystem.getPose().getTranslation(), new Rotation2d())),
+//                            m_driveSubsystem)
+//                .ignoringDisable(true));
 
     controller.a().whileTrue(m_armSubsystem.setShoulderPowerFactory(armPower.get()))
             .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0));
     controller.y().whileTrue(m_armSubsystem.setShoulderPowerFactory(-armPower.get()))
             .whileFalse(m_armSubsystem.setShoulderPowerFactory(0.0));
+    controller.b().whileTrue(m_armSubsystem.setShoulderPositionFactory(armPosition.get()))
+            .whileFalse(m_armSubsystem.setShoulderPositionFactory(0.0));
 
     controller.leftBumper().whileTrue(m_armSubsystem.setWristPowerFactory(wristPower.get()))
             .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
     controller.rightBumper().whileTrue(m_armSubsystem.setWristPowerFactory(-wristPower.get()))
             .whileFalse(m_armSubsystem.setWristPowerFactory(0.0));
+    controller.x().whileTrue(m_armSubsystem.setWristPositionFactory(wristPosition.get()))
+            .whileFalse(m_armSubsystem.setWristPositionFactory(0.0));
 
   }
 

src/main/java/frc/robot/subsystems/arm/ArmIOPrototype.java

@@ -1,23 +1,62 @@
 package frc.robot.subsystems.arm;
 
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.core.CoreTalonFX;
+import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
+import lib.properties.phoenix6.PidPropertyPublic;
 
 public class ArmIOPrototype implements ArmIO {
     private final TalonFX m_shoulder;
     private final TalonFX m_wrist;
+    private final PidPropertyPublic m_shoulderPID;
+    private final PidPropertyPublic m_wristPID;
+
     public ArmIOPrototype() {
         m_shoulder = new TalonFX(23);
         m_wrist = new TalonFX(24);
 
+        m_shoulderPID = new Phoenix6PidPropertyBuilder(
+                "Arm/Shoulder PID",
+                false,
+                m_shoulder,
+                0)
+                .addP(0.0)
+                .addI(0.0)
+                .addD(0.0)
+                .build();
+
+        m_wristPID = new Phoenix6PidPropertyBuilder(
+                "Arm/Wrist PID",
+                false,
+                m_wrist,
+                0)
+                .addP(0.0)
+                .addI(0.0)
+                .addD(0.0)
+                .build();
+
     }
 
     @Override
     public void setShoulderVoltage(double voltage){
         m_shoulder.setVoltage(voltage);
     }
 
+    @Override
+    public void setShoulderAngle(double degrees) {
+        final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
+        m_shoulder.setControl(m_request.withPosition(degrees / 360.0));
+    }
+
     @Override
     public void setWristVoltage(double voltage){
         m_wrist.setVoltage(voltage);
     }
-}
+
+    @Override
+    public void setWristAngle(double degrees) {
+        final PositionVoltage m_request = new PositionVoltage(0).withSlot(0);
+        m_wrist.setControl(m_request.withPosition(degrees / 360.0));
+    }
+}

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -10,19 +10,30 @@ public ArmSubsystem(ArmIO io) {
         m_io = io;
     }
 
-    public void setShoulderPower(double power){
-        m_io.setShoulderVoltage(power * 12.0);
-    }
-    public void setWristPower(double power){
-        m_io.setWristVoltage(power * 12.0);
+    public void setShoulderPower(double power) {m_io.setShoulderVoltage(power * 12.0);}
+
+    public void setShoulderPosition(double degrees) {m_io.setShoulderAngle(degrees);}
+
+    public void setWristPower(double power) {m_io.setWristVoltage(power * 12.0);}
+
+    public void setWristPosition(double degrees) {
+        m_io.setWristAngle(degrees);
     }
 
     public Command setShoulderPowerFactory(double power) {
         return runOnce(() -> setShoulderPower(power));
     }
 
+    public Command setShoulderPositionFactory(double degrees) {
+        return runOnce(() -> setShoulderPosition(degrees));
+    }
+
     public Command setWristPowerFactory(double power) {
         return runOnce(() -> setWristPower(power));
     }
+
+    public Command setWristPositionFactory(double degrees) {
+        return runOnce(() -> setWristPosition(degrees));
+    }
 }
 

src/main/java/frc/robot/subsystems/drive/module/Module.java

@@ -53,7 +53,7 @@ public Module(ModuleIO io) {
         case REAL, REPLAY -> {
           m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
           m_driveFeedback = new PIDController(0.05, 0.0, 0.0);
-          m_turnFeedback = new PIDController(7.0, 0.0, 0.0);
+          m_turnFeedback = new PIDController(0.1, 0.0, 0.0);
         }
         case SIM -> {
           m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
@@ -90,8 +90,7 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnPositionDegs(
-          m_turnFeedback.calculate(getAngle().getRadians(), m_angleSetpoint.getRadians()));
+      m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
 
       // Run closed loop drive control
       // Only allowed if closed loop turn control is running
@@ -104,10 +103,10 @@ public void periodic() {
         double adjustSpeedSetpoint = m_speedSetpoint * Math.cos(m_turnFeedback.getPositionError());
 
         // Run drive controller
-        double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS;
-        m_io.setDriveVelocityMPS(
-            m_driveFeedforward.calculate(velocityRadPerSec)
-                + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
+        m_io.setDriveVelocityMPS(adjustSpeedSetpoint);
+//        m_io.setDriveVelocityMPS(
+//            m_driveFeedforward.calculate(velocityRadPerSec)
+//                + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
       }
     }
 

src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java

@@ -55,7 +55,7 @@ public ModuleConstants(int id,
             DEFAULT_WHEEL_RADIUS_METERS * 2 * Math.PI,
             inverted,
             !inverted,
-            !inverted,
+            false,
             Rotation2d.fromDegrees(offsetDegs),
             id,
             ids[0], ids[1], ids[2],