Skip to content

Pure Pursuit - Group 2 #7

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
sbanerjee13 wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Pure Pursuit - Group 2 #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a91f8a9
Create PurePursuit.java
sbanerjee13 Dec 4, 2019
cec26fc
added PurePursuit.java
sbanerjee13 Dec 6, 2019
926f4e3
add pure pursuit methods to Drive
gradyw Dec 6, 2019
935c94b
drive.java and purepursuit.java
sbanerjee13 Dec 14, 2019
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
37 changes: 32 additions & 5 deletions OffSeasonBot2019/src/main/java/com/team2813/frc2019/subsystems/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import com.team2813.lib.config.MotorConfigs;
import com.team2813.lib.controls.Axis;
import com.team2813.lib.controls.Button;
import com.team2813.lib.purePursuit.PurePursuit;
import com.team2813.lib.sparkMax.CANSparkMaxWrapper;
import com.team2813.lib.sparkMax.SparkMaxException;
import com.team2813.lib.talon.CTREException;
Expand Down Expand Up @@ -58,9 +59,14 @@ public class Drive extends Subsystem {
private static double limelightDegrees = 0.0; // TODO: 10/05/2019 replace with actual Limelight angle
private static final double ALLOWABLE_LIMELIGHT_ERROR = 0.0; // TODO: 10/05/2019 replace with actual allowable angle error
private static final double MIN_AUTO_POS_CHANGE = 0.0; // TODO: 10/05/2019 tune
private static boolean purePursuit = false;
// private static final double MIN_AUTO_SPEED_FPS = 0.33; // TODO: 10/05/2019 tune
// private static final double MIN_AUTO_SPEED_ENCODER_TICKS = MIN_AUTO_SPEED_FPS * ENCODER_TICKS_PER_FOOT;

private PurePursuit path;
private double targetX = 0;
private double targetY = 0;

public enum TeleopDriveType {
ARCADE, CURVATURE
}
Expand Down Expand Up @@ -154,11 +160,16 @@ protected void teleopControls_() throws CTREException, SparkMaxException {
driveMode = DriveMode.OPEN_LOOP;
teleopDrive(TELEOP_DRIVE_TYPE);
} else {
driveMode = DriveMode.SMART_MOTION;
AUTO_BUTTON.whenPressed(() -> {
while (Math.abs(limelightDegrees) > ALLOWABLE_LIMELIGHT_ERROR)
autoDrive(limelightDegrees);
});
if (purePursuit) {
setPurePursuitVelocities();
} else {
startPurePursuit();
}
// driveMode = DriveMode.SMART_MOTION;
// AUTO_BUTTON.whenPressed(() -> {
// while (Math.abs(limelightDegrees) > ALLOWABLE_LIMELIGHT_ERROR)
// autoDrive(limelightDegrees);
// });
}
}

Expand Down Expand Up @@ -193,6 +204,22 @@ public synchronized void setBrakeMode(boolean brake) {
}
}

private void setPurePursuitVelocities() {
driveMode = DriveMode.VELOCITY;
// replace with pure pursuit method call to give enc values
right_demand = path.getRightVelocity();
left_demand = path.getLeftVelocity();
// need to set purePursuit to false when finished TODO: must be in if statement
purePursuit = !path.done();
}

private void startPurePursuit() {
purePursuit = true;
// add any code needed here to start pure pursuit
path = new PurePursuit(targetX, targetY);
path.calculate(8, 0.1, 2);
}

private enum DriveMode {
OPEN_LOOP(ControlType.kDutyCycle),
SMART_MOTION(ControlType.kSmartMotion),
Expand Down
Loading