ArmIO sim and minor cleanup

TitaniumTitans · Feb 28, 2024 · a13adef · a13adef
1 parent d4a1bd9
commit a13adef
Show file tree

Hide file tree

Showing 5 changed files with 103 additions and 44 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -179,19 +179,19 @@ private ArmConstants() {}
     public static final double WRIST_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (48.0 / 24.0);
     public static final double WRIST_CANCODER_MECHANISM_RATIO = (48.0 / 24.0);
 
-    public static final double WRIST_KP = 0.0;
-    public static final double WRIST_KI = 0.0;
-    public static final double WRIST_KD = 0.0;
-    public static final double WRIST_KS = 0.0;
+    public static final double WRIST_KP = 108.0;
+    public static final double WRIST_KI = 3.0;
+    public static final double WRIST_KD = 3.0;
+    public static final double WRIST_KS = 0.375;
     public static final double WRIST_KV = 0.0;
-    public static final double WRIST_KG = 0.0;
-
-    public static final double SHOULDER_KP = 0.0;
-    public static final double SHOULDER_KI = 0.0;
-    public static final double SHOULDER_KD = 0.0;
-    public static final double SHOULDER_KS = 0.0;
-    public static final double SHOULDER_KV = 0.0;
-    public static final double SHOULDER_KG = 0.0;
+    public static final double WRIST_KG = 0.35;
+
+    public static final double ARM_KP = 72.0;
+    public static final double ARM_KI = 6.0;
+    public static final double ARM_KD = 3.0;
+    public static final double ARM_KS = 0.375;
+    public static final double ARM_KV = 0.0;
+    public static final double ARM_KG = 0.375;
 
     public static final GosDoubleProperty WRIST_LOWER_LIMIT =
         new GosDoubleProperty(false, "Arm/WristLowerLimit", 30);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,47 +13,38 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.commands.DriveCommands;
-import frc.robot.subsystems.arm.ArmIO;
-import frc.robot.subsystems.arm.ArmIOKraken;
-import frc.robot.subsystems.arm.ArmIOPrototype;
-import frc.robot.subsystems.arm.ArmSubsystem;
+import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.subsystems.arm.*;
 import frc.robot.subsystems.climber.ClimberIO;
 import frc.robot.subsystems.climber.ClimberIOKraken;
-import frc.robot.subsystems.climber.ClimberIOPrototype;
 import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.GyroIO;
-import frc.robot.subsystems.drive.GyroIOPigeon1;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.module.ModuleIO;
 import frc.robot.subsystems.drive.module.ModuleIOSim;
 import frc.robot.subsystems.drive.module.ModuleIOTalonFX;
-import frc.robot.subsystems.shooter.*;
-import frc.robot.subsystems.vision.VisionSubsystem;
+import frc.robot.subsystems.shooter.ShooterIO;
+import frc.robot.subsystems.shooter.ShooterIOKraken;
+import frc.robot.subsystems.shooter.ShooterIOSim;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
 import org.littletonrobotics.junction.Logger;
-import lib.utils.FieldConstants;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -126,7 +117,7 @@ public RobotContainer() {
                 new ModuleIOSim(DriveConstants.BL_MOD_CONSTANTS),
                 new ModuleIOSim(DriveConstants.BR_MOD_CONSTANTS));
         m_shooter = new ShooterSubsystem(new ShooterIOSim());
-        m_armSubsystem = new ArmSubsystem(new ArmIO() {});
+        m_armSubsystem = new ArmSubsystem(new ArmIOSim() {});
         m_climber = new ClimberSubsystem(new ClimberIO() {});
       }
       default -> {
@@ -148,12 +139,12 @@ public RobotContainer() {
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
     // Set up feedforward characterization
-//    autoChooser.addOption(
-//        "Drive FF Characterization",
-//        new FeedForwardCharacterization(
-//                m_driveSubsystem,
-//                m_driveSubsystem::runCharacterizationVolts,
-//                m_driveSubsystem::getCharacterizationVelocity));
+    autoChooser.addOption(
+        "Drive FF Characterization",
+        new FeedForwardCharacterization(
+                m_driveSubsystem,
+                m_driveSubsystem::runCharacterizationVolts,
+                m_driveSubsystem::getCharacterizationVelocity));
 
     // Configure the button bindings
     configureButtonBindings();
@@ -174,18 +165,13 @@ private void configureButtonBindings() {
 
     controller.b().whileTrue(m_armSubsystem.setArmPositionFactory(180))
         .whileFalse(m_armSubsystem.setArmPowerFactory(0.0));
-//    controller.a().whileTrue(m_armSubsystem.setArmPositionFactory(90))
-//        .whileFalse(m_armSubsystem.setArmPowerFactory(0.0));
-//
-//    controller.a().onTrue(m_armSubsystem.resetEncoderFactory());
-//
+
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(
             m_driveSubsystem,
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(m_driveSubsystem::stopWithX, m_driveSubsystem));
     controller
         .start()
         .onTrue(

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java
@@ -0,0 +1,62 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.Constants.ArmConstants;
+
+public class ArmIOSim implements ArmIO{
+  private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(
+          DCMotor.getKrakenX60Foc(2), ArmConstants.ARM_SENSOR_MECHANISM_RATIO, 0.0060620304,
+          ArmConstants.ARM_LENGTH_METERS, -Math.PI * 30, Math.PI * 30, true, Units.degreesToRadians(45));
+
+  private final SingleJointedArmSim m_wristSim = new SingleJointedArmSim(
+          DCMotor.getKrakenX60Foc(2), ArmConstants.WRIST_SENSOR_MECHANISM_RATIO, 0.0060620304,
+          ArmConstants.WRIST_LENGTH_METERS, -Math.PI * 30, Math.PI * 30, true, Units.degreesToRadians(45));
+
+  double m_armAppliedOutput = 0.0;
+  double m_wristAppliedOutput = 0.0;
+
+  private final PIDController m_armController =
+          new PIDController(ArmConstants.ARM_KP, ArmConstants.ARM_KI, ArmConstants.ARM_KD);
+  private final PIDController m_wristController =
+          new PIDController(ArmConstants.WRIST_KP, ArmConstants.WRIST_KI, ArmConstants.WRIST_KD);
+
+  private static final double LOOP_PERIOD_SECS = 0.02;
+
+  @Override
+  public void updateInputs(ArmIOInputsAutoLogged inputs) {
+    m_armSim.update(LOOP_PERIOD_SECS);
+    m_wristSim.update(LOOP_PERIOD_SECS);
+
+    inputs.armPositionDegs = Units.radiansToDegrees(m_armSim.getAngleRads());
+    inputs.wristPositionDegs = Units.radiansToDegrees(m_wristSim.getAngleRads());
+
+    inputs.armVelocityDegsPerSecond = Units.radiansToDegrees(m_armSim.getVelocityRadPerSec());
+    inputs.wristVelocityDegsPerSecond = Units.radiansToDegrees(m_wristSim.getVelocityRadPerSec());
+  }
+
+  @Override
+  public void setArmAngle(double degrees) {
+    double setpointRots = Units.degreesToRotations(360);
+    m_armAppliedOutput = m_armController.calculate(Units.radiansToRotations(m_armSim.getAngleRads()), setpointRots);
+    m_armAppliedOutput = MathUtil.clamp(m_armAppliedOutput, -12.0, 12.0);
+    m_armSim.setInputVoltage(m_armAppliedOutput);
+  }
+
+  @Override
+  public void setWristAngle(double degrees, boolean track) {
+    double setpointRots = Units.degreesToRotations(360);
+    m_wristAppliedOutput = m_wristController.calculate(Units.radiansToRotations(m_wristSim.getAngleRads()), setpointRots);
+    m_wristAppliedOutput = MathUtil.clamp(m_wristAppliedOutput, -12.0, 12.0);
+    m_wristSim.setInputVoltage(m_wristAppliedOutput);
+  }
+
+  @Override
+  public void resetPosition() {
+    m_armSim.setState(0.0, 0.0);
+    m_wristSim.setState(Units.degreesToRadians(45.0), 0.0);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;
@@ -34,6 +35,9 @@ public enum ArmState {
   private ArmState m_desiredState = ArmState.DISABLED;
   private ArmState m_currentState = ArmState.DISABLED;
 
+  private final ArmVisualizer m_setpointVisualizer;
+  private final ArmVisualizer m_poseVisualizer;
+
   private final Supplier<Pose2d> m_poseSupplier;
 
   public ArmSubsystem(ArmIO io) {
@@ -50,6 +54,9 @@ public ArmSubsystem(ArmIO io, Supplier<Pose2d> supplier) {
     m_io.resetPosition();
 
     m_poseSupplier = supplier;
+
+    m_poseVisualizer = new ArmVisualizer("Current Arm Pose", Color.kFirstBlue);
+    m_setpointVisualizer = new ArmVisualizer("Current Arm Setpoint", Color.kFirstRed);
   }
 
   //TODO: Finite state machine logic
@@ -93,6 +100,10 @@ public void periodic() {
     }
 
     Logger.recordOutput("Arm/Wrist Gap", m_inputs.wristPositionDegs + m_inputs.armPositionDegs);
+
+    // Update arm visualizers
+    m_poseVisualizer.update(m_inputs.armPositionDegs);
+    m_setpointVisualizer.update(m_desiredArmPoseDegs);
   }
 
   public void handleState() {

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java
@@ -22,10 +22,10 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) {
     m_simLeft.update(LOOP_PERIOD_SECS);
     m_simRight.update(LOOP_PERIOD_SECS);
 
-    inputs.setTlVelocityRPM(m_simLeft.getAngularVelocityRadPerSec());
-    inputs.setTrVelocityRPM(m_simRight.getAngularVelocityRadPerSec());
-    inputs.setBlVelocityRPM(m_simLeft.getAngularVelocityRadPerSec());
-    inputs.setBrVelocityRPM(m_simRight.getAngularVelocityRadPerSec());
+    inputs.setTlVelocityRPM(m_simLeft.getAngularVelocityRPM());
+    inputs.setTrVelocityRPM(m_simRight.getAngularVelocityRPM());
+    inputs.setBlVelocityRPM(m_simLeft.getAngularVelocityRPM());
+    inputs.setBrVelocityRPM(m_simRight.getAngularVelocityRPM());
 
     inputs.setTlAppliedVolts(leftAppliedVolts);
     inputs.setTrAppliedVolts(rightAppliedVolts);