Skip to content

Completely New Pure Pursuit Implementation #212

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

Open
JIceberg wants to merge 8 commits into
base: dev
Choose a base branch
from
Open

Completely New Pure Pursuit Implementation #212

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
1b843fe
feat: implement new look-ahead algorithm
JIceberg Feb 22, 2022
33e65f6
fix: samples with new pure pursuit impl
JIceberg Feb 23, 2022
307a4e2
fix: remove out of date pure pursuit test cases
JIceberg Feb 23, 2022
6e44286
fix: incorrect loop logic in Path
JIceberg Feb 24, 2022
5f33d8f
remove reliance on DifferentialDrive in PurePursuitCommand
JIceberg Feb 24, 2022
59e70af
feat: streamline kinematics in sample
JIceberg Feb 24, 2022
032928b
Merge branch 'dev' into feature-new-pure-pursuit
JIceberg Feb 22, 2023
cedc52c
fix logic mistakes in implementation
JIceberg Feb 23, 2023
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
76 changes: 40 additions & 36 deletions core/src/main/java/com/arcrobotics/ftclib/command/PurePursuitCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,61 +1,65 @@
package com.arcrobotics.ftclib.command;

import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.geometry.Pose2d;
import com.arcrobotics.ftclib.purepursuit.Path;
import com.arcrobotics.ftclib.purepursuit.Waypoint;
import com.arcrobotics.ftclib.geometry.Translation2d;
import com.arcrobotics.ftclib.kinematics.wpilibkinematics.ChassisSpeeds;
import com.arcrobotics.ftclib.kinematics.wpilibkinematics.DifferentialDriveKinematics;
import com.arcrobotics.ftclib.kinematics.wpilibkinematics.DifferentialDriveWheelSpeeds;
import com.arcrobotics.ftclib.trajectory.Trajectory;
import com.arcrobotics.ftclib.trajectory.TrajectoryConfig;
import com.arcrobotics.ftclib.trajectory.purepursuit.PathFollower;

import java.util.function.BiConsumer;
import java.util.function.Supplier;

/**
* A command that makes use of the look-ahead algorithm for
* following a path derived from a trajectory.
*
* @author Jackson
* @see Path
*/
public class PurePursuitCommand extends CommandBase {

private MecanumDrive m_drive;
private OdometrySubsystem m_odometry;
private Path m_path;

public PurePursuitCommand(MecanumDrive drive, OdometrySubsystem odometry, Waypoint... waypoints) {
m_path = new Path(waypoints);
m_drive = drive;
m_odometry = odometry;
}

@Override
public void initialize() {
m_path.init();
}

public void addWaypoint(Waypoint waypoint) {
m_path.add(waypoint);
}

public void addWaypoints(Waypoint... waypoints) {
for (Waypoint waypoint : waypoints) this.addWaypoint(waypoint);
}
private DifferentialDriveKinematics kinematics;
private PathFollower follower;
private Translation2d poseTolerance;
private Supplier<Pose2d> currentRobotPose;
private BiConsumer<Double, Double> output;
private double lookAhead;

public void removeWaypointAtIndex(int index) {
m_path.remove(index);
public PurePursuitCommand(TrajectoryConfig config,
Trajectory trajectory,
DifferentialDriveKinematics kinematics,
double lookAheadDistance,
Translation2d poseTolerance,
Supplier<Pose2d> currentRobotPose,
BiConsumer<Double, Double> output) {
this.kinematics = kinematics;
this.currentRobotPose = currentRobotPose;
this.poseTolerance = poseTolerance;
this.output = output;
lookAhead = lookAheadDistance;
follower = new PathFollower(trajectory, config.getMaxVelocity());
}

/**
* Call this in a loop
*/
@Override
public void execute() {
Pose2d robotPose = m_odometry.getPose();
double[] motorSpeeds = m_path.loop(robotPose.getTranslation().getX(), robotPose.getTranslation().getY(), robotPose.getHeading());
m_drive.driveRobotCentric(motorSpeeds[0], motorSpeeds[1], motorSpeeds[2]);
Pose2d robotPose = currentRobotPose.get();
ChassisSpeeds chassisSpeeds = follower.getChassisSpeeds(robotPose, lookAhead);
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.normalize(1);
output.accept(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
}

@Override
public void end(boolean interrupted) {
m_drive.stop();
output.accept(0.0, 0.0);
}

@Override
public boolean isFinished() {
return m_path.isFinished();
Pose2d robotPose = currentRobotPose.get();
return !follower.isBusy(robotPose, poseTolerance);
}

}
55 changes: 0 additions & 55 deletions core/src/main/java/com/arcrobotics/ftclib/purepursuit/DecelerationController.java
View file
Open in desktop

This file was deleted.

Loading