More logging

src/main/java/frc/robot/Robot.java

@@ -157,6 +157,7 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    Logger.recordMetadata("Auto Command", m_autonomousCommand.getName());
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {

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

@@ -82,12 +82,15 @@
 
 import javax.management.InstanceNotFoundException;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentricFacingAngle;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.math.MathUtil;
@@ -99,6 +102,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -119,6 +123,7 @@
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
+  private final Field2d field;
   // declared as final in example code, but gives error in our code
   private SendableChooser<Command> autoChooser;
 
@@ -229,6 +234,11 @@ public class RobotContainer {
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
+        field = new Field2d();
+        SmartDashboard.putData("Field", field);
+        Logger.recordOutput("target pose", field.getObject("target pose").getPose());
+        // Logger.recordOutput("path", field.getObject("path").getPoses());
+
     switch (Constants.getRobotType()) {
       case WOODBOT:
         // Real robot, instantiate hardware IO implementations

src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java

@@ -181,6 +181,10 @@ public void updateInputs(AmpArmIOInputs inputs) {
     inputs.ampIntakeSensor = this.getIntakeSensor();
     inputs.zeroButton = this.getRawZeroButton();
     inputs.brakeButton = this.getRawBrakeButton();
+    inputs.armSetpoint = armMotor.getClosedLoopReference().getValueAsDouble();
+    inputs.wristSetpoint = wristMotor.getClosedLoopReference().getValueAsDouble();
+    inputs.armZeroed = armZeroed();
+    inputs.wristZeroed = wristZeroed();
   }
 
   @Override
@@ -193,6 +197,21 @@ public double getWristPosition() {
     return wristMotor.getPosition().getValueAsDouble();
   }
 
+  public boolean armZeroed() {
+    if (armMotor.getPosition().getValueAsDouble() == 0.0) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  public boolean wristZeroed() {
+    if (wristMotor.getPosition().getValueAsDouble() == 0.0) {
+      return true;
+    } else {
+      return false;
+    }
+  }
   @Override
   public void zeroWrist() {
     wristMotor.setPosition(0.0);

src/main/java/frc/robot/hardware/ClimberIOSparkMax.java

@@ -41,6 +41,9 @@ public class ClimberIOSparkMax implements ClimberIO {
     private final float rightRetractLimit = -57;
     private final float rightExtensionLimit = 60;
 
+    public double leftSetpoint = 0.0;
+    public double rightSetpoint = 0.0;
+
     private static class UnloadedConstants {
         static final double leftkP = 1.0;
         static final double leftkI = 0.0001;
@@ -148,6 +151,7 @@ public void stop() {
     public void setLeftHeight(double height, int pidSlot) { // height should be in inches
         height = height / POSITION_CONVERSION;
         leftPIDController.setReference(height, ControlType.kPosition, pidSlot);
+        leftSetpoint = height;
     }
 
     /**
@@ -157,6 +161,7 @@ public void setLeftHeight(double height, int pidSlot) { // height should be in i
     public void setRightHeight(double height, int pidSlot) {
         height = height / POSITION_CONVERSION;
         rightPIDController.setReference(height, ControlType.kPosition, pidSlot);
+        rightSetpoint = height;
     }
 
     @Override
@@ -213,5 +218,7 @@ public void updateInputs(ClimberIOInputs inputs) {
         inputs.climberRightVoltage = rightMotor.getAppliedOutput() * rightMotor.getBusVoltage();
         inputs.climberLeftDutyCycle = leftMotor.getAppliedOutput();
         inputs.climberRightDutyCycle = rightMotor.getAppliedOutput();
+        inputs.climberLeftSetpoint = leftSetpoint;
+        inputs.climberRightSetpoint = rightSetpoint;
     }
 }

src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java

@@ -27,7 +27,8 @@ public class FlywheelIOSparkFlex implements FlywheelIO {
     private final SparkPIDController rightPIDController = rightMotor.getPIDController();
 
     private final double VELOCITY_CONVERSION = 36.0/24.0; //24 motor rotations = 36 flywheel rotations (1.5)
-
+    public double leftSetpoint = 0.0;
+    public double rightSetpoint = 0.0;
 
   public FlywheelIOSparkFlex() {
     double kP = 0.0006;
@@ -73,12 +74,14 @@ public void setRight(double speed) {
 
   @Override
   public void setLeftReference(double rpm, ControlType kvelocity) {
-  leftPIDController.setReference(rpm, kvelocity);
+    leftPIDController.setReference(rpm, kvelocity);
+    leftSetpoint = rpm;
   }
 
   @Override
   public void setRightReference(double rpm, ControlType kvelocity) {
     rightPIDController.setReference(rpm, kvelocity);
+    rightSetpoint = rpm;
   }
 
   @Override
@@ -119,6 +122,7 @@ public void updateInputs(FlywheelIOInputs inputs) {
     inputs.flywheelRightVelocity = rightEncoder.getVelocity();
     inputs.flywheelLeftVoltage = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage();
     inputs.flywheelRightVoltage = rightMotor.getAppliedOutput() * rightMotor.getBusVoltage();
-
+    inputs.flywheelLeftSetpoint = leftSetpoint;
+    inputs.flywheelRightSetpoint = rightSetpoint;
   }
 }

src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java

@@ -22,10 +22,12 @@ public class IntakeIOSparkFlex implements IntakeIO {
     private final DigitalInput sideSensor = new DigitalInput(Constants.SIDE_SENSOR_PORT);
     private final DigitalInput intakeSensor = new DigitalInput(Constants.INTAKE_SENSOR_PORT);
     private final DigitalInput shooterSensor = new DigitalInput(Constants.SHOOTER_SENSOR_PORT);
+
+    public double setpoint;
 
     public IntakeIOSparkFlex(){
         sparkFlex.restoreFactoryDefaults(); 
-        sparkFlex.setInverted(false);
+        sparkFlex.setInverted(Constants.isCompBot() ? false : true);
 
         sparkFlex.setIdleMode(IdleMode.kBrake);
         sparkFlex.setSmartCurrentLimit(120, 50);
@@ -45,6 +47,7 @@ public void updateInputs(IntakeIOInputs inputs) {
         inputs.intakeVelocity = encoder.getVelocity();
         inputs.intakePosition = encoder.getPosition();
         inputs.shooterSensor = getShooterSensor();
+        inputs.intakeSetpoint = setpoint;
     }
 
     @Override
@@ -100,5 +103,6 @@ public void moveEncoder(double setpoint) {
     @Override
     public void setEncoderValue(double encoderPosition) {
         sparkFlex.getEncoder().setPosition(encoderPosition);
+        setpoint = encoderPosition;
     }
 }

src/main/java/frc/robot/hardware/LinkageIOTalonFX.java

@@ -66,6 +66,7 @@ public LinkageIOTalonFX(DigitalInput zeroButton, DigitalInput brakeButton) {
     this.zeroButton = zeroButton;
     this.brakeButton = brakeButton;
 
+
     final double kA = 0.0;
     final double kD = 0.0;
     final double kG = 0.0;
@@ -128,6 +129,14 @@ public LinkageIOTalonFX(DigitalInput zeroButton, DigitalInput brakeButton) {
     talonFX.getConfigurator().apply(talonFXConfiguration, 0.050);
   }
 
+  public boolean zeroed() {
+    if (talonFX.getPosition().getValueAsDouble() == 0.0) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
   private boolean getRawZeroButton(){
     return !this.zeroButton.get();
   }
@@ -159,6 +168,8 @@ public void updateInputs(LinkageIOInputs inputs) {
     inputs.linkagePosition = talonFX.getPosition().getValueAsDouble() * GEAR_RATIO;
     inputs.zeroButton = this.getRawZeroButton();
     inputs.brakeButton = this.getRawBrakeButton();
+    inputs.linkageSetpoint = talonFX.getClosedLoopReference().getValueAsDouble();
+    inputs.linkageZeroed = zeroed();
   }
 
   public void set(double speed) {

src/main/java/frc/robot/hardware/VisionIOLimelight.java

@@ -45,6 +45,7 @@ public void updateInputs(VisionIOInputs inputs) {
     inputs.tyBase = getTYBase();
     inputs.tyAdjusted = getTYAdjusted();
     inputs.pipeline = getPipeline();
+    inputs.botpose = getBotPose();
   }
 
   public double getTX() {

src/main/java/frc/robot/io/AmpArmIO.java

@@ -29,6 +29,11 @@ public static class AmpArmIOInputs {
     public boolean ampIntakeSensor = false;
     public boolean zeroButton = false;
     public boolean brakeButton = false;
+    public double armSetpoint = 0.0;
+    public double wristSetpoint = 0.0;
+    public boolean armZeroed = false;
+    public boolean wristZeroed = false;
+
   }
 
   public default void updateInputs(AmpArmIOInputs inputs) {}

src/main/java/frc/robot/io/ClimberIO.java

@@ -20,6 +20,8 @@ public static class ClimberIOInputs {
     public double climberRightVelocity = 0.0;
     public double climberLeftPosition = 0.0;
     public double climberRightPosition = 0.0;
+    public double climberLeftSetpoint = 0.0;
+    public double climberRightSetpoint = 0.0;
   }
 
   public default void updateInputs(ClimberIOInputs inputs) {

src/main/java/frc/robot/io/FlywheelIO.java

@@ -23,6 +23,8 @@ public static class FlywheelIOInputs {
     public double flywheelRightVelocity = 0.0;
     public double flywheelLeftPosition = 0.0;
     public double flywheelRightPosition = 0.0;
+    public double flywheelLeftSetpoint = 0.0;
+    public double flywheelRightSetpoint = 0.0;
   }
 
 public default void updateInputs(FlywheelIOInputs inputs) {}

src/main/java/frc/robot/io/IntakeIO.java

@@ -22,6 +22,7 @@ public static class IntakeIOInputs {
     // public double intakeSupplyCurrent = 0.0;
     public double intakePosition = 0.0;
     public double intakeVelocity = 0.0;
+    public double intakeSetpoint = 0.0;
   }
 
   public default void updateInputs(IntakeIOInputs inputs) {}

src/main/java/frc/robot/io/LinkageIO.java

@@ -24,6 +24,8 @@ public static class LinkageIOInputs {
     public double linkageSupplyCurrent =  0.0;
     public boolean zeroButton = false;
     public boolean brakeButton = false;
+    public double linkageSetpoint = 0.0;
+    public boolean linkageZeroed = false;
   }
 
   public default void updateInputs(LinkageIOInputs inputs) {}

src/main/java/frc/robot/io/VisionIO.java

@@ -16,6 +16,7 @@ public static class VisionIOInputs {
         public double tyAdjusted;
         public double tv;
         public double pipeline;
+        public Pose2d botpose;
     }
 
     public void updateInputs(VisionIOInputs inputs);
@@ -37,4 +38,5 @@ public static class VisionIOInputs {
     public void takeSnapshot();
 
     public void resetSnapshot();
+
 }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -377,5 +377,8 @@ public void periodic() {
         // Logger.recordOutput("Rotation2d", this.getPigeon2().getRotation2d());
         Logger.recordOutput("Swerve: CurrentState", this.getState().ModuleStates);
         Logger.recordOutput("Swerve: TargetState", this.getState().ModuleTargets);
+        Logger.recordOutput("Pigeon Yaw: ", this.getPigeon2().getYaw().getValueAsDouble());
+        Logger.recordOutput("Pigeon Pitch: ", this.getPigeon2().getPitch().getValueAsDouble());
+        Logger.recordOutput("Pigeon Roll: ", this.getPigeon2().getRoll().getValueAsDouble());
     }
 }

src/main/java/frc/robot/subsystems/Linkage.java

@@ -36,7 +36,7 @@ public class Linkage extends SubsystemBase {
   private final LinkageIO io;
   private final LinkageIOInputsAutoLogged inputs = new LinkageIOInputsAutoLogged();
   private double positionSetpoint;
-
+    
   private static final double STARTING_ANGLE = 50.0;
   static XboxController driverCont = new XboxController(0);