Skip to content

Commit

Permalink
Fix Autos!!!
Browse files Browse the repository at this point in the history
  • Loading branch information
@suryatho
suryatho committed Jan 31, 2024
1 parent b0a139e commit 1aabdb7
Show file tree
Hide file tree
Showing 10 changed files with 4,395 additions and 152 deletions.
4,325 changes: 4,307 additions & 18 deletions src/main/Potential Autos.chor
View file

Large diffs are not rendered by default.

68 changes: 32 additions & 36 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -35,6 +36,13 @@
import frc.robot.subsystems.superstructure.shooter.ShooterIO;
import frc.robot.subsystems.superstructure.shooter.ShooterIOSim;
import frc.robot.subsystems.superstructure.shooter.ShooterIOSparkMax;
import frc.robot.util.AllianceFlipUtil;
import frc.robot.util.trajectory.ChoreoTrajectoryReader;
import frc.robot.util.trajectory.Trajectory;
import java.io.File;
import java.util.Objects;
import java.util.Optional;
import java.util.function.Function;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

/**
Expand Down Expand Up @@ -153,35 +161,25 @@ public RobotContainer() {
.beforeStarting(() -> shooter.setCharacterizing(true))
.finallyDo(() -> shooter.setCharacterizing(false)));

autoChooser.addOption(
"Davis Auto Centerline first", TestAutos.davisAutoDefensive(drive, intake));
autoChooser.addOption("Davis Auto (4 note)", TestAutos.davisAuto(drive, intake));
autoChooser.addOption("4 Note Davis", TestAutos.fourNoteDavis(drive, intake));

// Testing autos paths
// Function<File, Optional<Command>> trajectoryCommandFactory =
// trajectoryFile -> {
// Optional<Trajectory> trajectory = ChoreoTrajectoryReader.generate(trajectoryFile);
// return trajectory.map(
// traj ->
// Commands.runOnce(
// () -> {
// robotState.resetPose(
// AllianceFlipUtil.apply(traj.getStartPose()),
// drive.getWheelPositions(),
// drive.getGyroYaw());
// drive.getMotionPlanner().setTrajectory(traj);
// },
// drive));
// };
// final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo");
// for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) {
// trajectoryCommandFactory
// .apply(trajectoryFile)
// .ifPresent(
// command -> {
// autoChooser.addOption(trajectoryFile.getName(), command);
// });
// }
Function<File, Optional<Command>> trajectoryCommandFactory =
trajectoryFile -> {
Optional<Trajectory> trajectory = ChoreoTrajectoryReader.generate(trajectoryFile);
return trajectory.map(
traj ->
Commands.runOnce(
() -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose())),
drive)
.andThen(drive.setTrajectoryCommand(traj)));
};
final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo");
for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) {
trajectoryCommandFactory
.apply(trajectoryFile)
.ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command));
}

// Configure the button bindings
configureButtonBindings();
Expand All @@ -200,15 +198,13 @@ private void configureButtonBindings() {
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive));
// controller
// .a()
// .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(),
// intake::running));
// controller
// .x()
// .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(),
// intake::running));
// controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive));
controller
.a()
.onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), intake::running));
controller
.x()
.onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), intake::running));
controller
.b()
.onTrue(
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import edu.wpi.first.math.numbers.N3;
import frc.robot.subsystems.drive.DriveConstants;
import java.util.NoSuchElementException;

import org.littletonrobotics.junction.AutoLogOutput;

public class RobotState {
Expand Down Expand Up @@ -142,8 +141,9 @@ public void resetPose(Pose2d initialPose) {

public Twist2d fieldVelocity() {
Translation2d linearFieldVelocity =
new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
return new Twist2d(linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
return new Twist2d(
linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
}

@AutoLogOutput(key = "Odometry/Robot")
Expand Down
21 changes: 21 additions & 0 deletions src/main/java/frc/robot/commands/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,34 @@
package frc.robot.commands.auto;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.FieldConstants;
import frc.robot.RobotState;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.superstructure.ShotCalculator;
import frc.robot.subsystems.superstructure.intake.Intake;
import frc.robot.util.AllianceFlipUtil;
import java.util.function.Supplier;

public class AutoCommands {
public static Command moveWhileShooting(Drive drive) {
return drive
.setHeadingCommand(AutoCommands::calculateShootHeading)
.andThen(Commands.waitSeconds(0.7))
.andThen(drive.disableHeadingCommand());
}

private static Rotation2d calculateShootHeading() {
Twist2d fieldVel = RobotState.getInstance().fieldVelocity();
return ShotCalculator.calculate(
AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getTranslation()),
RobotState.getInstance().getEstimatedPose().getTranslation(),
new Translation2d(fieldVel.dx, fieldVel.dy))
.goalHeading();
}

public static boolean inRegion(Supplier<Region> region) {
return region.get().contains(RobotState.getInstance().getEstimatedPose().getTranslation());
Expand Down
91 changes: 16 additions & 75 deletions src/main/java/frc/robot/commands/auto/TestAutos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,14 @@
import static frc.robot.util.trajectory.ChoreoTrajectoryReader.generate;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.FieldConstants;
import frc.robot.RobotState;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.DriveConstants;
import frc.robot.subsystems.superstructure.ShotCalculator;
import frc.robot.subsystems.superstructure.intake.Intake;
import frc.robot.util.AllianceFlipUtil;
import frc.robot.util.LoggedTunableNumber;
Expand All @@ -38,44 +35,8 @@ private static AutoCommands.CircularRegion centerlineIntakeRegion(int i) {

private static final RobotState robotState = RobotState.getInstance();

public static Command davisAutoDefensive(Drive drive, Intake intake) {
Trajectory trajectory = getTrajectory("DavisAutoDefensive.traj");

Command sequenceIntake =
Commands.sequence(
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(2))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(centerlineIntakeRegion(4))),
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(spikeIntakeRegion(1))));

var secondShot =
new AutoCommands.RectangularRegion(
new Translation2d(3.7, 7.1), 0.2, DriveConstants.drivetrainConfig.trackwidthY());
var thirdShot =
new AutoCommands.RectangularRegion(
new Translation2d(4.8, 6.2), 0.2, DriveConstants.drivetrainConfig.trackwidthY());

Command sequenceShots =
Commands.sequence(
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShot)),
moveWhileShooting(drive),
Commands.waitUntil(() -> !drive.getMotionPlanner().followingTrajectory()),
moveWhileShooting(drive));

return Commands.sequence(
resetPoseCommand(trajectory.getStartPose()),
Commands.waitSeconds(1.0), // Initial rev time for first shot
drive.followTrajectoryCommand(trajectory),
Commands.parallel(sequenceIntake, sequenceShots));
}

public static Command davisAuto(Drive drive, Intake intake) {
Trajectory trajectory = getTrajectory("DavisAutoOG.traj");
public static Command fourNoteDavis(Drive drive, Intake intake) {
Trajectory trajectory = getTrajectory("4NoteDavis.traj");

Command sequenceIntake =
Commands.sequence(
Expand All @@ -86,52 +47,32 @@ public static Command davisAuto(Drive drive, Intake intake) {
AutoCommands.intakeWhileInRegion(
intake, () -> AllianceFlipUtil.apply(centerlineIntakeRegion(4))));

var secondShot =
new AutoCommands.RectangularRegion(
new Translation2d(2.57, 7.3), DriveConstants.drivetrainConfig.trackwidthX(), 0.2);
var thirdShot =
new AutoCommands.RectangularRegion(
new Translation2d(3.7, 5.8), 0.2, DriveConstants.drivetrainConfig.trackwidthY());
var fourthShot =
new AutoCommands.RectangularRegion(
new Translation2d(4.34, 7.20), 0.2, DriveConstants.drivetrainConfig.trackwidthY());

var secondShotRegion =
new AutoCommands.RectangularRegion(new Translation2d(2.11, 6.37), 0.5, 0.1);
var thirdShotRegion =
new AutoCommands.RectangularRegion(new Translation2d(3.83, 5.99), 0.1, 0.5);
var fourthShotRegion =
new AutoCommands.RectangularRegion(new Translation2d(4.08, 7.18), 0.1, 0.5);
Command sequenceShots =
Commands.sequence(
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShot)),
moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(fourthShot)),
moveWhileShooting(drive));
AutoCommands.moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShotRegion)),
AutoCommands.moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShotRegion)),
AutoCommands.moveWhileShooting(drive),
AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(fourthShotRegion)),
AutoCommands.moveWhileShooting(drive));

return Commands.sequence(
resetPoseCommand(trajectory.getStartPose()),
Commands.waitSeconds(1.2), // Preload shot
drive.followTrajectoryCommand(trajectory), // start trajectory
drive.setTrajectoryCommand(trajectory),
Commands.parallel(sequenceIntake, sequenceShots));
}

private static Command resetPoseCommand(Pose2d pose) {
return Commands.runOnce(() -> robotState.resetPose(AllianceFlipUtil.apply(pose)));
}

private static Command moveWhileShooting(Drive drive) {
return drive
.setHeadingCommand(TestAutos::getShootHeading)
.andThen(Commands.waitSeconds(0.7))
.andThen(drive.disableHeadingCommand());
}

private static Rotation2d getShootHeading() {
Twist2d fieldVel = RobotState.getInstance().fieldVelocity();
return ShotCalculator.calculate(
AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getTranslation()),
RobotState.getInstance().getEstimatedPose().getTranslation(),
new Translation2d(fieldVel.dx, fieldVel.dy))
.goalHeading();
}

private static Trajectory getTrajectory(String fileName) {
File trajFile = new File(Filesystem.getDeployDirectory(), "/choreo/" + fileName);
return generate(trajFile).orElseThrow();
Expand Down
10 changes: 4 additions & 6 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.*;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -54,9 +53,6 @@ private interface MotionPlannerDelegate {
@Delegate(types = MotionPlannerDelegate.class)
private final DriveMotionPlanner motionPlanner;

@Getter(onMethod_ = @AutoLogOutput(key = "Odometry/FieldVelocity"))
private Twist2d fieldVelocity = new Twist2d();

private boolean characterizing = false;
private double characterizationVolts = 0.0;

Expand Down Expand Up @@ -128,7 +124,9 @@ public void periodic() {
// Get motion planner output
SwerveModuleState[] setpointStates =
motionPlanner.update(
Timer.getFPGATimestamp(), RobotState.getInstance().getEstimatedPose(), fieldVelocity);
Timer.getFPGATimestamp(),
RobotState.getInstance().getEstimatedPose(),
RobotState.getInstance().fieldVelocity());

SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
// Run robot
Expand Down Expand Up @@ -172,7 +170,7 @@ public double getCharacterizationVelocity() {
return driveVelocityAverage / 4.0;
}

public Command followTrajectoryCommand(Trajectory trajectory) {
public Command setTrajectoryCommand(Trajectory trajectory) {
return Commands.runOnce(() -> motionPlanner.setTrajectory(trajectory));
}

Expand Down
13 changes: 8 additions & 5 deletions src/main/java/frc/robot/subsystems/drive/DriveConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,14 @@
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.util.swerve.ModuleLimits;

/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */
public final class DriveConstants {
public static DrivetrainConfig drivetrainConfig =
switch (Constants.getRobot()) {
case SIMBOT, COMPBOT ->
new DrivetrainConfig(
Units.inchesToMeters(2.0),
Units.inchesToMeters(25.0),
Units.inchesToMeters(25.0),
Units.feetToMeters(13.05),
Expand All @@ -25,7 +25,6 @@ public final class DriveConstants {
43.97);
default ->
new DrivetrainConfig(
Units.inchesToMeters(2.0),
Units.inchesToMeters(26.0),
Units.inchesToMeters(26.0),
Units.feetToMeters(12.16),
Expand Down Expand Up @@ -102,6 +101,12 @@ public final class DriveConstants {
Mk4iReductions.TURN.reduction);
};

public static ModuleLimits moduleLimits =
new ModuleLimits(
drivetrainConfig.maxLinearVelocity(),
drivetrainConfig.maxLinearVelocity() * 5,
Units.degreesToRadians(1080.0));

public static TrajectoryConstants trajectoryConstants =
switch (Constants.getRobot()) {
case COMPBOT, RAINBOWT ->
Expand Down Expand Up @@ -132,12 +137,10 @@ public final class DriveConstants {

public static HeadingControllerConstants headingControllerConstants =
switch (Constants.getRobot()) {
case COMPBOT, RAINBOWT -> new HeadingControllerConstants(8.0, 0.0);
case SIMBOT -> new HeadingControllerConstants(4.0, 0.0);
default -> new HeadingControllerConstants(3.0, 0.0);
};

public record DrivetrainConfig(
double wheelRadius,
double trackwidthX,
double trackwidthY,
double maxLinearVelocity,
Expand Down
Loading

0 comments on commit 1aabdb7

Please sign in to comment.