diff --git a/.gitignore b/.gitignore index 37c6ba8..2a3bb90 100644 --- a/.gitignore +++ b/.gitignore @@ -44,6 +44,7 @@ *.wpilog advantagekit/logs logs +ctre_sim # BlueJ files *.ctxt diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index a2bed5b..2fd7e13 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -31,11 +32,16 @@ public class RobotState { MechanismLigament2d m_shooterPivotMechLower; MechanismLigament2d m_shooterPivotMechUpper; + MechanismLigament2d m_intakePivotMech; // advantagescope components private final boolean kComponentsEnabled = true; - private final Translation3d kShooterZeroTranslation = new Translation3d(0.017, 0.0, 0.415); - MechanismLigament2d m_intakePivotMech; + private final Pose3d kShooterZeroPose = + new Pose3d(new Translation3d(0.017, 0.0, 0.415), new Rotation3d()); + private final Pose3d kIntakeZeroPose = + new Pose3d( + new Translation3d(-0.269, -0.01, 0.2428), + new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(180), 0.0)); private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake) { // subsystems @@ -108,9 +114,20 @@ public void updateMechanism() { public void updateComponents() { Pose3d shooterPose = new Pose3d( - kShooterZeroTranslation, new Rotation3d(0, -m_shooter.getPivotAngle().getRadians(), 0)); + kShooterZeroPose.getTranslation(), + kShooterZeroPose + .getRotation() + .rotateBy(new Rotation3d(0, -m_shooter.getPivotAngle().getRadians(), 0))); + Pose3d intakePose = + new Pose3d( + kIntakeZeroPose.getTranslation(), + kIntakeZeroPose + .getRotation() + .rotateBy(new Rotation3d(0, m_intake.getPivotAngle().getRadians(), 0))); Logger.recordOutput("Components/ShooterPose", shooterPose); + Logger.recordOutput("Components/IntakePose", intakePose); + Logger.recordOutput("Components/Poses", new Pose3d[] {shooterPose, intakePose}); Logger.recordOutput("Components/ZeroPose", new Pose3d()); // for tuning config } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d351799..18ec08a 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,8 +42,11 @@ public void periodic() { Logger.processInputs("Intake/Roller", m_rollerInputs); Logger.recordOutput("Intake/Pivot/PIDVoltage", pivotPidVoltage); - Logger.recordOutput("Intake/Pivot/DesiredAngle", m_pivotController.getSetpoint().position); - Logger.recordOutput("Intake/CurrentAngle", Units.radiansToDegrees(pivotPidVoltage)); + Logger.recordOutput( + "Intake/Pivot/DesiredAngle", + Units.radiansToDegrees(m_pivotController.getSetpoint().position)); + Logger.recordOutput( + "Intake/Pivot/CurrentAngle", Units.radiansToDegrees(m_pivotInputs.curAngle)); } public void setRollerVoltage(double voltage) {