Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use interpolating tree map for calculating arm angle #16

Merged
merged 2 commits into from
Feb 16, 2024
Merged

Use interpolating tree map for calculating arm angle #16

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7818a28
Use interpolating tree map for calculating arm angle
suryatho Feb 15, 2024
5e782cd
Add compensation degrees
suryatho Feb 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 27 additions & 18 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,19 @@
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import java.util.NoSuchElementException;
import lombok.Getter;
import lombok.Setter;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.GeomUtil;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
Expand All @@ -29,7 +32,6 @@

@ExtensionMethod({GeomUtil.class})
public class RobotState {
// Pose Estimation
public record OdometryObservation(
SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}

Expand All @@ -40,11 +42,18 @@ public record AimingParameters(

private static final LoggedTunableNumber lookahead =
new LoggedTunableNumber("RobotState/lookaheadS", 0.0);
private static final double poseBufferSizeSeconds = 2.0;

private static LoggedTunableNumber shotHeightCompensation =
new LoggedTunableNumber("RobotState/CompensationMeters", 0.55);
/** Arm angle look up table key: meters, values: radians */
private static final InterpolatingDoubleTreeMap armAngleMap = new InterpolatingDoubleTreeMap();

private static final double poseBufferSizeSeconds = 2.0;
static {
armAngleMap.put(0.0, Units.degreesToRadians(90.0));
armAngleMap.put(10.0, Units.degreesToRadians(15.0));
armAngleMap.put(Double.MAX_VALUE, Units.degreesToRadians(15.0));
}

@Setter @Getter private double shotCompensationDegrees = 0.0;

private static RobotState instance;

Expand All @@ -59,7 +68,7 @@ public static RobotState getInstance() {
private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
private final Matrix<N3, N1> qStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
// odometry
// Odometry
private final SwerveDriveKinematics kinematics;
private SwerveDriveWheelPositions lastWheelPositions =
new SwerveDriveWheelPositions(
Expand All @@ -72,6 +81,7 @@ public static RobotState getInstance() {
private Rotation2d lastGyroAngle = new Rotation2d();
private Twist2d robotVelocity = new Twist2d();

/** Cached latest aiming parameters. Calculated in {@code getAimingParameters()} */
private AimingParameters latestParameters = null;

private RobotState() {
Expand Down Expand Up @@ -155,7 +165,7 @@ public void addVisionObservation(VisionObservation observation) {
}

public void addVelocityData(Twist2d robotVelocity) {
this.latestParameters = null;
latestParameters = null;
this.robotVelocity = robotVelocity;
}

Expand Down Expand Up @@ -191,15 +201,14 @@ public AimingParameters getAimingParameters() {
latestParameters =
new AimingParameters(
targetVehicleDirection,
new Rotation2d(
targetDistance - ArmConstants.armOrigin.getX(),
FieldConstants.Speaker.centerSpeakerOpening.getZ()
- ArmConstants.armOrigin.getY()
+ shotHeightCompensation.get()),
Rotation2d.fromRadians(
armAngleMap.get(targetDistance) + Units.degreesToRadians(shotCompensationDegrees)),
feedVelocity);
Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading);
Logger.recordOutput("RobotState/AimingParameters/ArmAngle", latestParameters.armAngle);
Logger.recordOutput("RobotState/AimingParameters/DriveFeedVelocityRadPerS", feedVelocity);
Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading());
Logger.recordOutput("RobotState/AimingParameters/ArmAngle", latestParameters.armAngle());
Logger.recordOutput(
"RobotState/AimingParameters/DriveFeedVelocityRadPerS",
latestParameters.driveFeedVelocity());
return latestParameters;
}

Expand All @@ -213,15 +222,15 @@ public void resetPose(Pose2d initialPose) {
poseBuffer.clear();
}

@AutoLogOutput(key = "Odometry/FieldVelocity")
@AutoLogOutput(key = "RobotState/FieldVelocity")
public Twist2d fieldVelocity() {
Translation2d linearFieldVelocity =
new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
return new Twist2d(
linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
}

@AutoLogOutput(key = "Odometry/EstimatedPose")
@AutoLogOutput(key = "RobotState/EstimatedPose")
public Pose2d getEstimatedPose() {
return estimatedPose;
}
Expand All @@ -243,7 +252,7 @@ public Pose2d getPredictedPose(double translationLookaheadS, double rotationLook
robotVelocity.dtheta * rotationLookaheadS));
}

@AutoLogOutput(key = "Odometry/OdometryPose")
@AutoLogOutput(key = "RobotState/OdometryPose")
public Pose2d getOdometryPose() {
return odometryPose;
}
Expand Down
Loading