From 46238bf3a4bdaca787883dc53785c59558e0ca47 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sun, 24 Mar 2024 07:18:43 -0700 Subject: [PATCH 01/13] set point logging attempt --- .../frc/robot/commands/LinkageToAmpHandoff.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java b/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java index d915885c..236eaebf 100644 --- a/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java +++ b/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java @@ -27,6 +27,10 @@ public class LinkageToAmpHandoff extends Command { private States state; + double linkageSetPoint; + double ampArmSetPoint; + double ampWristSetPoint; + // private double ampThreshold; private final Timer timer = new Timer(); @@ -66,6 +70,7 @@ public void execute() { switch (state) { case LINKAGE_DOWN: linkage.setAngle(0.0, ampArm); + linkageSetPoint = 0.0; if (linkage.getAngle() < 2.0) { state = States.SET_ARM; } @@ -73,6 +78,8 @@ public void execute() { case SET_ARM: ampArm.setArm(-45.0, linkage); ampArm.setWrist(45.0); + ampArmSetPoint = -45.0; + ampWristSetPoint = 45.0; if (Math.abs(ampArm.getArmPosition() + 45.0) < 2.0 && Math.abs(ampArm.getWristPosition() - 45.0) < 2.0) { timer.start(); state = States.INTAKING; @@ -91,6 +98,7 @@ public void execute() { break; case HAS_NOTE: ampArm.setArm(-8.0, linkage); + ampArmSetPoint = -8.0; if (ampArm.getArmPosition() > -10.0) { state = States.RETRACTED; } @@ -98,11 +106,17 @@ public void execute() { case RETRACTED: linkage.setAngle(174.0, ampArm); ampArm.setWrist(82.0); + linkageSetPoint = 174.0; + ampWristSetPoint = 82.0; if (Math.abs(linkage.getAngle() - 174) < 1.0) { done = true; } break; } + + Logger.recordOutput("Linkage Set Point", linkageSetPoint); + Logger.recordOutput("Amp Arm Set Point", ampArmSetPoint); + Logger.recordOutput("Amp Wrist Set Point", ampWristSetPoint); } // Called once the command ends or is interrupted. From 66a366b97f38cbe3fd8f0bf01b2a3cee3dc91fe1 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sun, 24 Mar 2024 13:23:26 -0700 Subject: [PATCH 02/13] setpoint logging --- .../frc/robot/commands/LinkageToAmpHandoff.java | 14 -------------- .../java/frc/robot/hardware/AmpArmIOTalonFX.java | 2 ++ .../java/frc/robot/hardware/LinkageIOTalonFX.java | 1 + src/main/java/frc/robot/io/AmpArmIO.java | 2 ++ src/main/java/frc/robot/io/LinkageIO.java | 1 + 5 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java b/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java index 236eaebf..d915885c 100644 --- a/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java +++ b/src/main/java/frc/robot/commands/LinkageToAmpHandoff.java @@ -27,10 +27,6 @@ public class LinkageToAmpHandoff extends Command { private States state; - double linkageSetPoint; - double ampArmSetPoint; - double ampWristSetPoint; - // private double ampThreshold; private final Timer timer = new Timer(); @@ -70,7 +66,6 @@ public void execute() { switch (state) { case LINKAGE_DOWN: linkage.setAngle(0.0, ampArm); - linkageSetPoint = 0.0; if (linkage.getAngle() < 2.0) { state = States.SET_ARM; } @@ -78,8 +73,6 @@ public void execute() { case SET_ARM: ampArm.setArm(-45.0, linkage); ampArm.setWrist(45.0); - ampArmSetPoint = -45.0; - ampWristSetPoint = 45.0; if (Math.abs(ampArm.getArmPosition() + 45.0) < 2.0 && Math.abs(ampArm.getWristPosition() - 45.0) < 2.0) { timer.start(); state = States.INTAKING; @@ -98,7 +91,6 @@ public void execute() { break; case HAS_NOTE: ampArm.setArm(-8.0, linkage); - ampArmSetPoint = -8.0; if (ampArm.getArmPosition() > -10.0) { state = States.RETRACTED; } @@ -106,17 +98,11 @@ public void execute() { case RETRACTED: linkage.setAngle(174.0, ampArm); ampArm.setWrist(82.0); - linkageSetPoint = 174.0; - ampWristSetPoint = 82.0; if (Math.abs(linkage.getAngle() - 174) < 1.0) { done = true; } break; } - - Logger.recordOutput("Linkage Set Point", linkageSetPoint); - Logger.recordOutput("Amp Arm Set Point", ampArmSetPoint); - Logger.recordOutput("Amp Wrist Set Point", ampWristSetPoint); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java index 8a031655..d429edcc 100644 --- a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java +++ b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java @@ -181,6 +181,8 @@ 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(); } @Override diff --git a/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java b/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java index d480ff29..647a7189 100644 --- a/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java +++ b/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java @@ -159,6 +159,7 @@ 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(); } public void set(double speed) { diff --git a/src/main/java/frc/robot/io/AmpArmIO.java b/src/main/java/frc/robot/io/AmpArmIO.java index ff14bb36..fc9bcd5b 100644 --- a/src/main/java/frc/robot/io/AmpArmIO.java +++ b/src/main/java/frc/robot/io/AmpArmIO.java @@ -29,6 +29,8 @@ 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 default void updateInputs(AmpArmIOInputs inputs) {} diff --git a/src/main/java/frc/robot/io/LinkageIO.java b/src/main/java/frc/robot/io/LinkageIO.java index a49dbde6..791d0259 100644 --- a/src/main/java/frc/robot/io/LinkageIO.java +++ b/src/main/java/frc/robot/io/LinkageIO.java @@ -24,6 +24,7 @@ public static class LinkageIOInputs { public double linkageSupplyCurrent = 0.0; public boolean zeroButton = false; public boolean brakeButton = false; + public double linkageSetpoint = 0.0; } public default void updateInputs(LinkageIOInputs inputs) {} From 96ddfeafa9ac4cf64495cd440c18a3f2aa409e9b Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Mon, 25 Mar 2024 20:11:17 -0700 Subject: [PATCH 03/13] setpoints for TalonFX motors --- src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java b/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java index 01219f1e..ce259816 100644 --- a/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java +++ b/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java @@ -45,6 +45,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.intakeVelocity = encoder.getVelocity(); inputs.intakePosition = encoder.getPosition(); inputs.shooterSensor = getShooterSensor(); + // inputs.intakeSetpoint = ??? } @Override From 2629d794b7b6a138ddd4a0eae4733c8e48c0d785 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Fri, 29 Mar 2024 17:16:29 -0700 Subject: [PATCH 04/13] setpoint logging attempt --- .../java/frc/robot/hardware/FlywheelIOSparkFlex.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java b/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java index fe7b9fc7..dcb6f590 100644 --- a/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java +++ b/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java @@ -73,7 +73,7 @@ public void setRight(double speed) { @Override public void setLeftReference(double rpm, ControlType kvelocity) { - leftPIDController.setReference(rpm, kvelocity); + leftPIDController.setReference(rpm, kvelocity); } @Override @@ -81,6 +81,15 @@ public void setRightReference(double rpm, ControlType kvelocity) { rightPIDController.setReference(rpm, kvelocity); } + public double getLeftReference(double rpm) { + return rpm; + } + + public double getRightReference(double rpm) { + return rpm; + } + + @Override public void stopLeftMotor() { leftMotor.stopMotor(); @@ -119,6 +128,5 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelRightVelocity = rightEncoder.getVelocity(); inputs.flywheelLeftVoltage = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); inputs.flywheelRightVoltage = rightMotor.getAppliedOutput() * rightMotor.getBusVoltage(); - } } \ No newline at end of file From f5307088b1f9010369835ca8fcf9da5b1f979ca0 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sat, 30 Mar 2024 13:55:56 -0700 Subject: [PATCH 05/13] getting Auto --- src/main/java/frc/robot/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49d4701a..9073112a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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) { From 40486bc0ba3d6c00c063ead6213279670141a2f4 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sat, 30 Mar 2024 14:01:38 -0700 Subject: [PATCH 06/13] copy and paste of pathplanner documentation for path logging --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ef72882..49d82b79 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,6 +88,7 @@ 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 +100,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 +121,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 autoChooser; @@ -229,6 +232,27 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + field = new Field2d(); + SmartDashboard.putData("Field", field); + + // Logging callback for current robot pose + PathPlannerLogging.setLogCurrentPoseCallback((pose) -> { + // Do whatever you want with the pose here + field.setRobotPose(pose); + }); + + // Logging callback for target robot pose + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + // Do whatever you want with the pose here + field.getObject("target pose").setPose(pose); + }); + + // Logging callback for the active path, this is sent as a list of poses + PathPlannerLogging.setLogActivePathCallback((poses) -> { + // Do whatever you want with the poses here + field.getObject("path").setPoses(poses); + }); + switch (Constants.getRobotType()) { case WOODBOT: // Real robot, instantiate hardware IO implementations From c852808f0d02a4b29c2f1056ba803126bd04b442 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sat, 30 Mar 2024 18:04:27 -0700 Subject: [PATCH 07/13] setpoint and path logging --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/hardware/ClimberIOSparkMax.java | 7 +++++++ .../frc/robot/hardware/FlywheelIOSparkFlex.java | 16 ++++++---------- .../frc/robot/hardware/IntakeIOSparkFlex.java | 7 +++++-- src/main/java/frc/robot/io/ClimberIO.java | 2 ++ src/main/java/frc/robot/io/FlywheelIO.java | 2 ++ src/main/java/frc/robot/io/IntakeIO.java | 1 + 7 files changed, 25 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 49d82b79..0fdff9d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -232,7 +232,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - field = new Field2d(); + field = new Field2d(); SmartDashboard.putData("Field", field); // Logging callback for current robot pose @@ -252,7 +252,7 @@ public RobotContainer() { // Do whatever you want with the poses here field.getObject("path").setPoses(poses); }); - + switch (Constants.getRobotType()) { case WOODBOT: // Real robot, instantiate hardware IO implementations diff --git a/src/main/java/frc/robot/hardware/ClimberIOSparkMax.java b/src/main/java/frc/robot/hardware/ClimberIOSparkMax.java index feb8c23b..f3298c40 100644 --- a/src/main/java/frc/robot/hardware/ClimberIOSparkMax.java +++ b/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; } } diff --git a/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java b/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java index dcb6f590..3e358473 100644 --- a/src/main/java/frc/robot/hardware/FlywheelIOSparkFlex.java +++ b/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; @@ -74,22 +75,15 @@ public void setRight(double speed) { @Override public void setLeftReference(double rpm, ControlType kvelocity) { leftPIDController.setReference(rpm, kvelocity); + leftSetpoint = rpm; } @Override public void setRightReference(double rpm, ControlType kvelocity) { rightPIDController.setReference(rpm, kvelocity); + rightSetpoint = rpm; } - public double getLeftReference(double rpm) { - return rpm; - } - - public double getRightReference(double rpm) { - return rpm; - } - - @Override public void stopLeftMotor() { leftMotor.stopMotor(); @@ -128,5 +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; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java b/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java index ce259816..abfa0367 100644 --- a/src/main/java/frc/robot/hardware/IntakeIOSparkFlex.java +++ b/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,7 +47,7 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.intakeVelocity = encoder.getVelocity(); inputs.intakePosition = encoder.getPosition(); inputs.shooterSensor = getShooterSensor(); - // inputs.intakeSetpoint = ??? + inputs.intakeSetpoint = setpoint; } @Override @@ -101,5 +103,6 @@ public void moveEncoder(double setpoint) { @Override public void setEncoderValue(double encoderPosition) { sparkFlex.getEncoder().setPosition(encoderPosition); + setpoint = encoderPosition; } } diff --git a/src/main/java/frc/robot/io/ClimberIO.java b/src/main/java/frc/robot/io/ClimberIO.java index bee43b96..de65d4a0 100644 --- a/src/main/java/frc/robot/io/ClimberIO.java +++ b/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) { diff --git a/src/main/java/frc/robot/io/FlywheelIO.java b/src/main/java/frc/robot/io/FlywheelIO.java index 74e1684b..f201f725 100644 --- a/src/main/java/frc/robot/io/FlywheelIO.java +++ b/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) {} diff --git a/src/main/java/frc/robot/io/IntakeIO.java b/src/main/java/frc/robot/io/IntakeIO.java index 5b150f47..4226bed9 100644 --- a/src/main/java/frc/robot/io/IntakeIO.java +++ b/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) {} From 0f8c6fb033cae9d5f9748f782840486a5536825d Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sat, 30 Mar 2024 18:47:38 -0700 Subject: [PATCH 08/13] zero and botpose --- .../frc/robot/hardware/AmpArmIOTalonFX.java | 17 +++++++++++++++++ .../frc/robot/hardware/LinkageIOTalonFX.java | 10 ++++++++++ .../frc/robot/hardware/VisionIOLimelight.java | 1 + src/main/java/frc/robot/io/AmpArmIO.java | 3 +++ src/main/java/frc/robot/io/LinkageIO.java | 1 + src/main/java/frc/robot/io/VisionIO.java | 2 ++ src/main/java/frc/robot/subsystems/Linkage.java | 2 +- 7 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java index d429edcc..264e8eb9 100644 --- a/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java +++ b/src/main/java/frc/robot/hardware/AmpArmIOTalonFX.java @@ -183,6 +183,8 @@ public void updateInputs(AmpArmIOInputs inputs) { inputs.brakeButton = this.getRawBrakeButton(); inputs.armSetpoint = armMotor.getClosedLoopReference().getValueAsDouble(); inputs.wristSetpoint = wristMotor.getClosedLoopReference().getValueAsDouble(); + inputs.armZeroed = armZeroed(); + inputs.wristZeroed = wristZeroed(); } @Override @@ -195,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); diff --git a/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java b/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java index 647a7189..763a5097 100644 --- a/src/main/java/frc/robot/hardware/LinkageIOTalonFX.java +++ b/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(); } @@ -160,6 +169,7 @@ public void updateInputs(LinkageIOInputs inputs) { inputs.zeroButton = this.getRawZeroButton(); inputs.brakeButton = this.getRawBrakeButton(); inputs.linkageSetpoint = talonFX.getClosedLoopReference().getValueAsDouble(); + inputs.linkageZeroed = zeroed(); } public void set(double speed) { diff --git a/src/main/java/frc/robot/hardware/VisionIOLimelight.java b/src/main/java/frc/robot/hardware/VisionIOLimelight.java index cb365594..e9aaaf84 100644 --- a/src/main/java/frc/robot/hardware/VisionIOLimelight.java +++ b/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() { diff --git a/src/main/java/frc/robot/io/AmpArmIO.java b/src/main/java/frc/robot/io/AmpArmIO.java index fc9bcd5b..e16df147 100644 --- a/src/main/java/frc/robot/io/AmpArmIO.java +++ b/src/main/java/frc/robot/io/AmpArmIO.java @@ -31,6 +31,9 @@ public static class AmpArmIOInputs { 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) {} diff --git a/src/main/java/frc/robot/io/LinkageIO.java b/src/main/java/frc/robot/io/LinkageIO.java index 791d0259..2fc320ea 100644 --- a/src/main/java/frc/robot/io/LinkageIO.java +++ b/src/main/java/frc/robot/io/LinkageIO.java @@ -25,6 +25,7 @@ public static class LinkageIOInputs { public boolean zeroButton = false; public boolean brakeButton = false; public double linkageSetpoint = 0.0; + public boolean linkageZeroed = false; } public default void updateInputs(LinkageIOInputs inputs) {} diff --git a/src/main/java/frc/robot/io/VisionIO.java b/src/main/java/frc/robot/io/VisionIO.java index 3c21c692..8571e4ea 100644 --- a/src/main/java/frc/robot/io/VisionIO.java +++ b/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(); + } diff --git a/src/main/java/frc/robot/subsystems/Linkage.java b/src/main/java/frc/robot/subsystems/Linkage.java index 4e01708d..55b7014a 100644 --- a/src/main/java/frc/robot/subsystems/Linkage.java +++ b/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); From 4182ebb508f81ceca97ed4d60a05dd3d2f2eb8a5 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Sat, 30 Mar 2024 19:47:03 -0700 Subject: [PATCH 09/13] pigeon --- .../java/frc/robot/subsystems/CommandSwerveDrivetrain.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 195a88c4..b8ffa4b5 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/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()); } } \ No newline at end of file From f850972d968d8995f82c6321daf72e4c245233ba Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Mon, 1 Apr 2024 16:44:27 -0700 Subject: [PATCH 10/13] path planner fix attempt --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0fdff9d8..acc0f160 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,6 +82,8 @@ 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; @@ -245,6 +247,7 @@ public RobotContainer() { PathPlannerLogging.setLogTargetPoseCallback((pose) -> { // Do whatever you want with the pose here field.getObject("target pose").setPose(pose); + Logger.recordOutput("target pose", field.getObject("target pose").getPose()); }); // Logging callback for the active path, this is sent as a list of poses From 49dc189d080de9940ef77a6640787258b1ede9ff Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Mon, 1 Apr 2024 17:26:42 -0700 Subject: [PATCH 11/13] idk man --- src/main/java/frc/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index acc0f160..1c3f3375 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -248,6 +248,7 @@ public RobotContainer() { // Do whatever you want with the pose here field.getObject("target pose").setPose(pose); Logger.recordOutput("target pose", field.getObject("target pose").getPose()); + Logger.recordOutput("path", (String[]) field.getObject("path").getPoses().toArray()); }); // Logging callback for the active path, this is sent as a list of poses From 52ad1c4caeee6225af24d7298480ac5b907e0263 Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Mon, 1 Apr 2024 17:43:46 -0700 Subject: [PATCH 12/13] path planning lol --- src/main/java/frc/robot/RobotContainer.java | 22 ++------------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1c3f3375..675c6293 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -236,26 +236,8 @@ public class RobotContainer { public RobotContainer() { field = new Field2d(); SmartDashboard.putData("Field", field); - - // Logging callback for current robot pose - PathPlannerLogging.setLogCurrentPoseCallback((pose) -> { - // Do whatever you want with the pose here - field.setRobotPose(pose); - }); - - // Logging callback for target robot pose - PathPlannerLogging.setLogTargetPoseCallback((pose) -> { - // Do whatever you want with the pose here - field.getObject("target pose").setPose(pose); - Logger.recordOutput("target pose", field.getObject("target pose").getPose()); - Logger.recordOutput("path", (String[]) field.getObject("path").getPoses().toArray()); - }); - - // Logging callback for the active path, this is sent as a list of poses - PathPlannerLogging.setLogActivePathCallback((poses) -> { - // Do whatever you want with the poses here - field.getObject("path").setPoses(poses); - }); + Logger.recordOutput("target pose", field.getObject("target pose").getPose()); + Logger.recordOutput("path", (String[]) field.getObject("path").getPoses().toArray()); switch (Constants.getRobotType()) { case WOODBOT: From 8f04166f25684cb5a2635ad9378d35b9f3ec06fe Mon Sep 17 00:00:00 2001 From: Xtr3m3Boi5 <122415457+annashimshock@users.noreply.github.com> Date: Mon, 1 Apr 2024 18:38:24 -0700 Subject: [PATCH 13/13] i dont know what im doing --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 675c6293..6f0135e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -237,7 +237,7 @@ public RobotContainer() { field = new Field2d(); SmartDashboard.putData("Field", field); Logger.recordOutput("target pose", field.getObject("target pose").getPose()); - Logger.recordOutput("path", (String[]) field.getObject("path").getPoses().toArray()); + // Logger.recordOutput("path", field.getObject("path").getPoses()); switch (Constants.getRobotType()) { case WOODBOT: