Skip to content
This repository has been archived by the owner on Jan 13, 2025. It is now read-only.

Commit

Permalink
reformatted odometry and cleaned up code
Browse files Browse the repository at this point in the history
  • Loading branch information
@qntmcube
qntmcube committed Nov 14, 2024
1 parent 7f836f6 commit 04fe6aa
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 193 deletions.
10 changes: 10 additions & 0 deletions src/main/java/com/team766/odometry/KalmanFilter.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
package com.team766.odometry;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;

public class KalmanFilter {


}
231 changes: 51 additions & 180 deletions src/main/java/com/team766/odometry/Odometry.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,9 @@
package com.team766.odometry;

import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.hardware.CANcoder;
import com.team766.hal.GyroReader;
import com.team766.hal.MotorController;
import com.team766.library.RateLimiter;
import com.team766.logging.Category;
import com.team766.logging.Logger;
import com.team766.logging.Severity;
import edu.wpi.first.math.geometry.Pose2d;
import com.team766.robot.common.mechanisms.SwerveModule;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;

Expand All @@ -26,19 +19,19 @@ public class Odometry {
private RateLimiter odometryLimiter;

private GyroReader gyro;
private MotorController[] motorList;
private SwerveModule[] moduleList;
// The order of CANCoders should be the same as in motorList
private CANcoder[] CANCoderList;
private int motorCount;
private int moduleCount;

private Pose2d[] prevPositions;
private Pose2d[] currPositions;
private double[] prevEncoderValues;
private double[] currEncoderValues;
private Rotation2d[] prevWheelRotation;
private Rotation2d[] wheelRotationChange;

private double[] prevDriveDisplacement;
private double[] driveDisplacementChange;


private Rotation2d gyroPosition;

private Pose2d currentPosition;

// In meters
private double wheelCircumference;
Expand All @@ -60,141 +53,73 @@ public class Odometry {
*/
public Odometry(
GyroReader gyro,
MotorController[] motors,
CANcoder[] CANCoders,
SwerveModule[] moduleList,
Translation2d[] wheelLocations,
double wheelCircumference,
double gearRatio,
int encoderToRevolutionConstant) {

this.gyro = gyro;
odometryLimiter = new RateLimiter(RATE_LIMITER_TIME);
motorList = motors;
CANCoderList = CANCoders;
motorCount = motorList.length;
// log("Motor count " + motorCount);
prevPositions = new Pose2d[motorCount];
currPositions = new Pose2d[motorCount];
prevEncoderValues = new double[motorCount];
currEncoderValues = new double[motorCount];
this.moduleList = moduleList;
moduleCount = moduleList.length;

prevWheelRotation = new Rotation2d[moduleCount];
wheelRotationChange = new Rotation2d[moduleCount];

currentPosition = new Pose2d();
prevDriveDisplacement = new double[moduleCount];
driveDisplacementChange = new double[moduleCount];

wheelPositions = wheelLocations;
this.wheelCircumference = wheelCircumference;
this.gearRatio = gearRatio;
this.encoderToRevolutionConstant = encoderToRevolutionConstant;
currentPosition = new Pose2d(0, 0, new Rotation2d());

for (int i = 0; i < motorCount; i++) {
prevPositions[i] = new Pose2d(0, 0, new Rotation2d());
currPositions[i] = new Pose2d(0, 0, new Rotation2d());
prevEncoderValues[i] = 0;
currEncoderValues[i] = 0;
}
}
for (int i = 0; i < moduleCount; i++) {
prevWheelRotation[i] = new Rotation2d();
wheelRotationChange[i] = new Rotation2d();

/**
* Sets the current position of the robot to Point P
* @param P The point to set the current robot position to
*/
public void setCurrentPosition(final Pose2d point) {

currentPosition = point;
// Logger.get(Category.PROCEDURES)
// .logRaw(Severity.INFO, "Set Current Position to: " + point.toString());
for (int i = 0; i < motorCount; i++) {
prevPositions[i] =
currentPosition.plus(new Transform2d(wheelPositions[i], new Rotation2d()));
currPositions[i] =
currentPosition.plus(new Transform2d(wheelPositions[i], new Rotation2d()));
prevDriveDisplacement[i] = 0;
driveDisplacementChange[i] = 0;
}
// log("Current Position: " + currentPosition.toString());
}

/**
* Updates the odometry encoder values to the robot encoder values.
*/
private void setCurrentEncoderValues() {
for (int i = 0; i < motorCount; i++) {
prevEncoderValues[i] = currEncoderValues[i];
currEncoderValues[i] = motorList[i].getSensorPosition();
// Optional<Alliance> alliance = DriverStation.getAlliance();
// currEncoderValues[i] *=
// ((alliance.isPresent() && (alliance.get() == Alliance.Blue)) ? 1 : -1);
private void updateDisplacementAndRotation() {
for (int i = 0; i < moduleCount; i++) {
Rotation2d currentWheelRotation = gyroPosition.plus(moduleList[i].getSteerAngle());
wheelRotationChange[i] = currentWheelRotation.minus(prevWheelRotation[i]);
prevWheelRotation[i] = currentWheelRotation;

double currentDriveDisplacement = moduleList[i].getDriveDisplacement();
driveDisplacementChange[i] = currentDriveDisplacement - prevDriveDisplacement[i];
prevDriveDisplacement[i] = currentDriveDisplacement;
}
}

private static Vector2D rotate(Vector2D v, double angle) {
return new Vector2D(
v.getX() * Math.cos(angle) - Math.sin(angle) * v.getY(),
v.getY() * Math.cos(angle) + v.getX() * Math.sin(angle));
}

/**
* Updates the position of each wheel of the robot by assuming each wheel moved in an arc.
*/
private void updateCurrentPositions() {
Rotation2d rotationChange;
public Translation2d predictCurrentPositionChange() {
double radius;
double deltaX;
double deltaY;
gyroPosition = Rotation2d.fromDegrees(gyro.getAngle());

/*
Point slopeFactor = new Point(Math.sqrt(Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))),
Math.sqrt(Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.sin(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) * Math.cos(Math.toRadians(Robot.gyro.getGyroPitch())) + Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroYaw())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll())) * Math.cos(Math.toRadians(Robot.gyro.getGyroRoll()))));
*/

for (int i = 0; i < motorCount; i++) {

StatusSignal<Double> positionStatus = CANCoderList[i].getAbsolutePosition();
if (!positionStatus.getStatus().isOK()) {
Logger.get(Category.ODOMETRY)
.logData(
Severity.WARNING,
"Unable to read CANCoder: %s",
positionStatus.getStatus().toString());
continue;
}
double sumX = 0;
double sumY = 0;

double absolutePosition = 360 * positionStatus.getValueAsDouble();

// prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 *
// DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) *
// Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)),
// currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI /
// motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) /
// motorCount)), currPositions[i].getHeading());
// This following line only works if the average of wheel positions is (0,0)

prevPositions[i] =
new Pose2d(
currentPosition
.plus(new Transform2d(wheelPositions[i], new Rotation2d()))
.getTranslation(),
currPositions[i].getRotation());
// SmartDashboard.putNumber(
// "early curr rotation", currPositions[i].getRotation().getDegrees());
// SmartDashboard.putString("prev rotation direct", prevPositions[i].toString());
currPositions[i] =
new Pose2d(
currPositions[i].getTranslation(),
gyroPosition.plus(Rotation2d.fromDegrees(absolutePosition)));

rotationChange = currPositions[i].getRotation().minus(prevPositions[i].getRotation());
// SmartDashboard.putNumber("curr rotation",
// currPositions[i].getRotation().getDegrees());
// SmartDashboard.putNumber("prev rotation",
// prevPositions[i].getRotation().getDegrees());
// SmartDashboard.putNumber("rotation change", rotationChange.getDegrees());
updateDisplacementAndRotation();

for (int i = 0; i < moduleCount; i++) {

double yaw = Math.toRadians(gyro.getAngle());
// SmartDashboard.putNumber("odom yaw", yaw);
double roll = Math.toRadians(gyro.getRoll());
double pitch = Math.toRadians(gyro.getPitch());

double w = Math.toRadians(absolutePosition);
double w = moduleList[i].getSteerAngle().getRadians();
Vector2D u =
new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch));
Vector2D v =
Expand All @@ -211,82 +136,28 @@ private void updateCurrentPositions() {
// double oldWheelX;
// double oldWheelY;

// estimates the bot moved in a circle to calculate new position
if (Math.abs(rotationChange.getDegrees()) != 0) {
radius =
180
* (currEncoderValues[i] - prevEncoderValues[i])
/ (Math.PI * rotationChange.getDegrees());
// could def make this math cleaner w/rotation functions
deltaX = radius * Math.sin(Math.toRadians(rotationChange.getDegrees()));
deltaY = radius * (1 - Math.cos(Math.toRadians(rotationChange.getDegrees())));

if (Math.abs(wheelRotationChange[i].getDegrees()) != 0) {
// estimates the bot moved in a circle to calculate new position
radius = driveDisplacementChange[i] / wheelRotationChange[i].getRadians();

wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY));
deltaX = radius * Math.sin(wheelRotationChange[i].getRadians());
deltaY = radius * (1 - Math.cos(wheelRotationChange[i].getRadians()));

// oldWheelX = ((Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX -
// Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) *
// slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO *
// ENCODER_TO_REVOLUTION_CONSTANT));
// oldWheelY = ((Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX +
// Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) *
// slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO *
// ENCODER_TO_REVOLUTION_CONSTANT));
wheelMotion = a.scalarMultiply(deltaX).add(b.scalarMultiply(-deltaY));

} else {
wheelMotion = a.scalarMultiply((currEncoderValues[i] - prevEncoderValues[i]));

// oldWheelX = ((currEncoderValues[i] - prevEncoderValues[i]) *
// Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() *
// WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
// oldWheelY = ((currEncoderValues[i] - prevEncoderValues[i]) *
// Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() *
// WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
wheelMotion = a.scalarMultiply(driveDisplacementChange[i]);

}
wheelMotion =
wheelMotion.scalarMultiply(
wheelCircumference / (gearRatio * encoderToRevolutionConstant));
// wheelMotion = rotate(wheelMotion, Math.toRadians(gyroPosition));
// log("Difference: " + (oldWheelX - wheelMotion.getX()) + ", " + (oldWheelY -
// wheelMotion.getY()) + "Old Method: " + oldWheelX + ", " + oldWheelY + "Current
// Method: " + wheelMotion.getX() + ", " + wheelMotion.getY());
// log("Current: " + currPositions[i] + " Motion: " + wheelMotion + " New: " +
// currPositions[i].add(wheelMotion));

currPositions[i] =
new Pose2d(
currPositions[i].getX() + wheelMotion.getX(),
currPositions[i].getY() + wheelMotion.getY(),
currPositions[i].getRotation());
}
}

/**
* Calculates the position of the robot by finding the average of the wheel positions.
*/
private void findRobotPosition() {
double sumX = 0;
double sumY = 0;
for (int i = 0; i < motorCount; i++) {
sumX += currPositions[i].getX();
sumY += currPositions[i].getY();
// log("sumX: " + sumX + " Motor Count: " + motorCount + " CurrentPosition: " +
// currPositions[i]);
}
// x and y are inverted to follow directional conventions
currentPosition = new Pose2d(sumX / motorCount, sumY / motorCount, gyroPosition);
}

// Intended to be placed inside Robot.drive.run()
public void run() {
if (odometryLimiter.next()) {
setCurrentEncoderValues();
updateCurrentPositions();
findRobotPosition();
// log(currentPosition.toString());

sumX += wheelMotion.getX();
sumY += wheelMotion.getY();
}
}

public Pose2d getCurrPosition() {
return currentPosition;
return new Translation2d(sumX / moduleCount, sumY / moduleCount);
}
}
Loading

0 comments on commit 04fe6aa

Please sign in to comment.