Skip to content

Commit

Permalink
Make fixes for NovelOdometry and DistanceMovement, fix points list an…
Browse files Browse the repository at this point in the history 
…d add turnPoints logic to PathFinder
  • Loading branch information
@liambridgers
liambridgers committed Apr 20, 2024
1 parent f3c98af commit 25b49b2
Show file tree
Hide file tree
Showing 9 changed files with 7,236 additions and 1,026 deletions.
7 changes: 2 additions & 5 deletions TeamCode/src/main/java/centerstage/auto/OdometryMovementTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,13 @@ public class OdometryMovementTest extends AutonomousOpMode {
private NovelMecanumDriver driver;
private DistanceMovement movement;
private OdometryUpdater updater;

@Override
public void initialize() {
this.c = new CenterStageRobotConfiguration(this.hardwareMap);
this.driver = this.c.createDriver(Constants.Coefficients.PRODUCTION_COEFFICIENTS);
this.odometry = this.c.createOdometry();
c.imu.resetYaw();
this.movement = new DistanceMovement(driver, odometry, c.imu);
this.movement = new DistanceMovement(driver, odometry, c.imu, 0, 5);
this.updater = new OdometryUpdater(odometry);
}

Expand All @@ -37,8 +36,6 @@ public void run() {
this.c.br.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.c.bl.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

movement.transform(-29, 3);

this.driver.stop();
movement.move(0, 15, 180);
}
}
22 changes: 11 additions & 11 deletions TeamCode/src/main/java/centerstage/auto/VirtualFieldAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,16 +91,16 @@ private Vector3D getSpikePlacementPosition() {

private Vector3D getParkPosition() {
if (alliance == Alliance.RED && startSide == StartSide.APRIL_TAG_SIDE) {
return new Vector3D(133, 134, 0);
return new Vector3D(131, 132, 0);
}
if (alliance == Alliance.RED && startSide == StartSide.BACKDROP_SIDE) {
return new Vector3D(133, 134, 0);
return new Vector3D(131, 132, 0);
}
if (alliance == Alliance.BLUE && startSide == StartSide.APRIL_TAG_SIDE) {
return new Vector3D(9, 134, 0);
return new Vector3D(12, 132, 0);
}
if (alliance == Alliance.BLUE && startSide == StartSide.BACKDROP_SIDE) {
return new Vector3D(9, 134, 0);
return new Vector3D(12, 132, 0);
}
return Vector3D.ZERO;
}
Expand All @@ -109,17 +109,17 @@ private Vector3D getParkPosition() {
public void run() {
updater.start();

try {
SpikePosition spikePosition = getSpikePosition();
// try {
// SpikePosition spikePosition = getSpikePosition();

virtualField.pathTo(getSpikePlacementPosition());
// virtualField.pathTo(getSpikePlacementPosition());

placeSpike(spikePosition);
// placeSpike(spikePosition);

virtualField.pathTo(getParkPosition());
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
// } catch (InterruptedException e) {
// throw new RuntimeException(e);
// }
}

private void placeSpike(SpikePosition position) throws InterruptedException {
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/com/pocolifo/robobase/novel/OdometryCoefficientSet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,6 @@ public OdometryCoefficientSet(double rightCoefficient, double leftCoefficient, d
}

public OdometryCoefficientSet() {
this(1, 1, 1);
this(-1, -1, -1);
}
}
64 changes: 28 additions & 36 deletions TeamCode/src/main/java/com/pocolifo/robobase/novel/hardware/NovelOdometry.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,42 +26,28 @@ public NovelOdometry(OdometryCoefficientSet coefficients, NovelEncoder rightEnco

// Adapted from https://gm0.org/en/latest/docs/software/concepts/odometry.html
public void update() {
// Update to fetch current wheel positions
double newLeftWheelPos = leftEncoder.getCurrentInches();
double newRightWheelPos = rightEncoder.getCurrentInches();
double newPerpendicularWheelPos = perpendicularEncoder.getCurrentInches();

// Calculate deltas for each wheel position
double deltaLeftWheelPos = coefficients.leftCoefficient * (newLeftWheelPos - leftWheelPos);
double deltaRightWheelPos = coefficients.rightCoefficient * (newRightWheelPos - rightWheelPos);
double deltaPerpendicularWheelPos = coefficients.perpendicularCoefficient * (newPerpendicularWheelPos - perpendicularWheelPos);

// Calculate the change in orientation (phi)
double averageMovementLeftRight = (deltaLeftWheelPos - deltaRightWheelPos) / 2;
// System.out.println("Average Movement Left Right: " + averageMovementLeftRight);
double phi = averageMovementLeftRight / (Constants.Odometry.ODOMETRY_LATERAL_WHEEL_DISTANCE);
// System.out.println("phi: " + phi);

// Calculate the average forward movement
double deltaMiddlePos = (deltaLeftWheelPos + deltaRightWheelPos) / 2.0;
// System.out.println("deltaMiddlePos: " + deltaMiddlePos);

// Correct deltaPerpendicularPos to eliminate the rotational component
// System.out.println("deltaPerpendicularWheelPos: " + deltaPerpendicularWheelPos);
double deltaPerpendicularPos = deltaPerpendicularWheelPos - averageMovementLeftRight;
// System.out.println("deltaPerpendicularPos: " + deltaPerpendicularPos);

// Assuming currentOrientation is the robot's orientation at the start of this calculation
double initialOrientation = relativePose.getHeading(AngleUnit.RADIANS); // Assuming this is the orientation at the start
double newOrientation = initialOrientation + phi;

//double heading = (phi + this.relativePose.getHeading(AngleUnit.RADIANS) + this.relativePose.getHeading(AngleUnit.RADIANS)) / 2;

double currentOrientation = (initialOrientation + newOrientation) / 2.0;
// Calculate global movement deltas
double deltaX = deltaMiddlePos * Math.sin(currentOrientation) - deltaPerpendicularPos * Math.cos(currentOrientation);
double deltaY = deltaMiddlePos * Math.cos(currentOrientation) + deltaPerpendicularPos * Math.sin(currentOrientation);

// Get new wheel positions
double newLeftWheelPos = this.leftEncoder.getCurrentInches();
double newRightWheelPos = this.rightEncoder.getCurrentInches();
double newPerpendicularWheelPos = this.perpendicularEncoder.getCurrentInches();

// Get changes in odometer wheel positions since last update
double deltaLeftWheelPos = this.coefficients.leftCoefficient * (newLeftWheelPos - this.leftWheelPos);
double deltaRightWheelPos = this.coefficients.rightCoefficient * (newRightWheelPos - this.rightWheelPos); // Manual adjustment for inverted odometry wheel
double deltaPerpendicularWheelPos = this.coefficients.perpendicularCoefficient * (newPerpendicularWheelPos - this.perpendicularWheelPos);

double phi = (deltaLeftWheelPos - deltaRightWheelPos) / Constants.Odometry.ODOMETRY_LATERAL_WHEEL_DISTANCE;
double deltaMiddlePos = (deltaLeftWheelPos + deltaRightWheelPos) / 2d;
double deltaPerpendicularPos = deltaPerpendicularWheelPos - Constants.Odometry.ODOMETRY_ROTATIONAL_WHEEL_OFFSET * phi;

// Heading of movement is assumed average between last known and current rotation
// CURRENT ROTATION LAST SAVED ROTATION
// double currentRotation = phi + this.relativePose.getHeading(AngleUnit.RADIANS);
// double lastRotation = this.relativePose.getHeading(AngleUnit.RADIANS);
// double averageRotationOverObservationPeriod = (currentRotation + lastRotation) / 2;
double heading = phi + this.relativePose.getHeading(AngleUnit.RADIANS);
double deltaX = -(deltaMiddlePos * Math.sin(-heading) + deltaPerpendicularPos * Math.cos(-heading));
double deltaY = deltaMiddlePos * Math.cos(-heading) - deltaPerpendicularPos * Math.sin(-heading);

this.relativePose = this.relativePose.add(new Pose(deltaX, deltaY, phi, AngleUnit.RADIANS));

Expand All @@ -81,4 +67,10 @@ public void resetRelativePose() {
this.rightWheelPos = this.rightEncoder.getCurrentInches();
this.perpendicularWheelPos = this.perpendicularEncoder.getCurrentInches();
}

public Pose getAbsolutePose(double startRotation) {
double absoluteX = this.relativePose.getX() * Math.cos(startRotation) + this.relativePose.getY() * Math.sin(startRotation);
double absoluteY = (this.relativePose.getY() * Math.cos(startRotation) + this.relativePose.getX() * Math.sin(startRotation));
return new Pose(absoluteX, absoluteY, this.relativePose.getHeading(AngleUnit.RADIANS), AngleUnit.RADIANS);
}
}
45 changes: 38 additions & 7 deletions TeamCode/src/main/java/com/pocolifo/robobase/reconstructor/PathFinder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,16 @@

public class PathFinder {
private final Set<Vector3D> points;
private final Set<Vector3D> turnPoints;

public PathFinder(String filePath) throws IOException {
public PathFinder(String pointsFilePath, String turnPointsFilePath) throws IOException {
this.points = new HashSet<>();
readVector3DsFromFile(filePath);
this.turnPoints = new HashSet<>();
readVector3DsFromFile(pointsFilePath, points);
readVector3DsFromFile(turnPointsFilePath, turnPoints);
}

private void readVector3DsFromFile(String filePath) throws IOException {
private void readVector3DsFromFile(String filePath, Set pointsSet) throws IOException {
URL resource = this.getClass().getResource("/" + filePath);
if (resource == null) {
throw new FileNotFoundException("File " + filePath + " is not found in /" + filePath);
Expand All @@ -27,12 +30,12 @@ private void readVector3DsFromFile(String filePath) throws IOException {
String[] parts = line.split(" ");
int x = Integer.parseInt(parts[0]);
int y = Integer.parseInt(parts[1]);
points.add(new Vector3D(x, y, 0));
pointsSet.add(new Vector3D(x, y, 0));
}
}
}

public List<Vector3D> findPath(Vector3D start, Vector3D goal) {
public Path findPath(Vector3D start, Vector3D goal) {
start = new Vector3D(start.getX(), start.getY(), 0);
goal = new Vector3D(goal.getX(), goal.getY(), 0);
if (!points.contains(start)) {
Expand All @@ -54,7 +57,8 @@ public List<Vector3D> findPath(Vector3D start, Vector3D goal) {
Vector3D current = openSet.poll().point;
if (current.equals(goal)) {
List<Vector3D> totalPath = reconstructPath(cameFrom, current);
return filterPath(totalPath);
Vector3D turnPoint = getSafeTurnPoint(totalPath);
return new Path(filterPath(totalPath), turnPoint);
}

for (Vector3D neighbor : getNeighbors(current)) {
Expand All @@ -70,7 +74,16 @@ public List<Vector3D> findPath(Vector3D start, Vector3D goal) {
}
}

return Collections.emptyList(); // Path not found
return new Path(Collections.emptyList(), null); // Path not found
}

private Vector3D getSafeTurnPoint(List<Vector3D> path) {
for (Vector3D point : path) {
if (turnPoints.contains(point)) {
return point;
}
}
return null;
}

private List<Vector3D> filterPath(List<Vector3D> path) {
Expand Down Expand Up @@ -171,4 +184,22 @@ public int hashCode() {
return Objects.hash(point);
}
}

public class Path {
private List<Vector3D> points;
private Vector3D turnPoint;

public Path(List<Vector3D> points, Vector3D turnPoint) {
this.points = points;
this.turnPoint = turnPoint;
}

public List<Vector3D> getPoints() {
return points;
}

public Vector3D getTurnPoint() {
return turnPoint;
}
}
}
80 changes: 50 additions & 30 deletions TeamCode/src/main/java/com/pocolifo/robobase/virtualfield/DistanceMovement.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import com.pocolifo.robobase.reconstructor.Pose;
import com.qualcomm.robotcore.hardware.IMU;

import org.apache.commons.math3.geometry.Vector;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

Expand All @@ -15,24 +16,28 @@
* @see NovelMecanumDriver
*/
public class DistanceMovement {
private static final double PRECISION_IN = 4;
private static final double PERCISION_DEG = 1;
private static final double SPEED = 5;
private static final double PRECISION_IN = 0.1;
private static final double PERCISION_DEG = 0.2;
private final NovelMecanumDriver movementController;
private final NovelOdometry odometry;
private final IMU imu;
private double positionalDifference = -1;
private double rotationaldifference = -1;
private double startRotation;
private Vector3D velocity;
private double speed;

public DistanceMovement(NovelMecanumDriver movementController, NovelOdometry odometry, IMU imu) {
public DistanceMovement(NovelMecanumDriver movementController, NovelOdometry odometry, IMU imu, double startRotation, double speed) {
this.movementController = movementController;
this.odometry = odometry;
this.imu = imu;
this.startRotation = startRotation;
this.speed = speed;
}

private void updatePositionalAndRotationalDifference(Vector3D target, Vector3D position) {
positionalDifference = Math.sqrt(Math.pow(position.getX() - target.getX(), 2) + Math.pow(position.getY() - target.getY(), 2));
rotationaldifference = getRotationMovement(position.getZ(), target.getZ());
rotationaldifference = Math.abs(DistanceMovement.getRotationMovement(position.getZ(), target.getZ()));
}

private void moveBy(Vector3D movement) {
Expand All @@ -43,7 +48,7 @@ private void moveBy(Vector3D movement) {
while (positionalDifference > PRECISION_IN || rotationaldifference > PERCISION_DEG) {
position = getPosition();
updatePositionalAndRotationalDifference(target, position);
Vector3D velocity = getNewVelocity(position, target, SPEED);
Vector3D velocity = getNewVelocity(position, target, speed);

movementController.setVelocity(velocity);
}
Expand Down Expand Up @@ -81,9 +86,12 @@ private Vector3D poseToVector3D(Pose pose) {
return new Vector3D(pose.getX(), pose.getY(), pose.getHeading(AngleUnit.DEGREES));
}

private Vector3D getPosition() {
Vector3D odometryPosition = poseToVector3D(odometry.getRelativePose());
double imuRotation = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
public Vector3D getPosition() {
Vector3D odometryPosition = poseToVector3D(odometry.getAbsolutePose(startRotation));
double imuRotation = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (imuRotation < 0) {
imuRotation += 360;
}
return new Vector3D(odometryPosition.getX(), odometryPosition.getY(), imuRotation);
}

Expand All @@ -97,31 +105,43 @@ private static double getRotationMovement(double current, double target) {
return delta;
}

private static Vector3D getNewVelocity(Vector3D position, Vector3D target, double speed) {
double horizontalMovement = target.getX() - position.getX();
double verticalMovement = target.getY() - position.getY();
private Vector3D getNewVelocity(Vector3D position, Vector3D target, double speed) {

double horizontalMovement = target.getX() - position.getX();
double verticalMovement = target.getY() - position.getY();
double rotationTarget = getRotationMovement(position.getZ(), target.getZ());

double currentRotationRadians = Math.toRadians(position.getZ()) + this.startRotation;

double rotationTarget = (target.getZ() - target.getZ()) % 360;
if (rotationTarget < -180) {
rotationTarget += 360;
} else if (rotationTarget > 180) {
rotationTarget -= 360;
}
double horizontal = (horizontalMovement * Math.cos(currentRotationRadians) + verticalMovement * -Math.sin(currentRotationRadians));
double vertical = (verticalMovement * -Math.cos(-currentRotationRadians) + horizontalMovement * Math.sin(-currentRotationRadians));

// Convert current rotation from degrees to radians for trigonometric functions
double currentRotationRadians = Math.toRadians(position.getZ());
Vector3D absoluteVelocity = new Vector3D(vertical, horizontal, rotationTarget / 4); // Lower rotation so it does not block x/y movement completely when normalized
this.velocity = absoluteVelocity;

// Calculate the absolute movements based on the current rotation
double absoluteHorizontalMovement = horizontalMovement * Math.cos(currentRotationRadians) - verticalMovement * Math.sin(currentRotationRadians);
double absoluteVerticalMovement = horizontalMovement * Math.sin(currentRotationRadians) + verticalMovement * Math.cos(currentRotationRadians);
if (absoluteVelocity.equals(Vector3D.ZERO)) {
return Vector3D.ZERO;
}

// Create a new vector for the absolute velocity and normalize it
Vector3D absoluteVelocity = new Vector3D(absoluteVerticalMovement, absoluteHorizontalMovement * -1, rotationTarget);
// Slow speed down when close to target
if (rotationaldifference + positionalDifference < speed / 5) {
speed = 3;
}

if (absoluteVelocity.equals(Vector3D.ZERO)) {
return absoluteVelocity;
}
absoluteVelocity = absoluteVelocity.normalize().scalarMultiply(speed);

return absoluteVelocity;
}

return absoluteVelocity.normalize().scalarMultiply(speed);
}
public double getRotationaldifference() {
return rotationaldifference;
}

public double getPositionalDifference() {
return positionalDifference;
}

public Vector3D getVelocity() {
return velocity;
}
}
Loading

0 comments on commit 25b49b2

Please sign in to comment.