Skip to content

Commit

Permalink
Apply Prettier format
Browse files Browse the repository at this point in the history
  • Loading branch information
github-actions committed Aug 4, 2024
1 parent a621c45 commit 89acd2d
Show file tree
Hide file tree
Showing 3 changed files with 184 additions and 147 deletions.
81 changes: 53 additions & 28 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
package frc.robot;

import com.pathplanner.lib.util.PIDConstants;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -323,31 +322,57 @@ public static final class NoteSim {
public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8);
public static final double OUT_OF_FIELD_MARGIN = .025;
}
public static final class AutoAlign {
public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0);
public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0);
public static final double TIME_TO_FEED = 1.0; // seconds
public static final double TIME_FOR_SCORING = .5; // seconds

public static final Pose2d redAmpSpeakerPose =
new Pose2d(15.59, 6.644, new Rotation2d(Math.toRadians(120.5)));
public static final Pose2d blueAmpSpeakerPose =
new Pose2d(0.909, 6.644, new Rotation2d(Math.toRadians(55.5)));
public static final Pose2d redSourceSpeakerPose =
new Pose2d(15.636, 4.39, new Rotation2d(Math.toRadians(-122.5)));
public static final Pose2d blueSourceSpeakerPose =
new Pose2d(0.864, 4.39, new Rotation2d(Math.toRadians(-62.5)));
public static final Pose2d redAmpPose =
new Pose2d(14.7, 7.72, new Rotation2d(Math.toRadians(-90)));
public static final Pose2d blueAmpPose =
new Pose2d(1.85, 7.72, new Rotation2d(Math.toRadians(-90)));

public static final Pose2d blueSpeakerPose =
new Pose2d(1.45, 5.50, new Rotation2d(Math.toRadians(0)));
public static final Pose2d redSpeakerPose =
new Pose2d(15.1, 5.50, new Rotation2d(Math.toRadians(180)));

public static final double TRANSLATION_ERROR_MARGIN = 0.4;
public static final double ROTATION_ERROR_MARGIN = 0.4;
}

public static final class AutoAlign {

public static final PIDConstants TRANSLATIONAL_PID = new PIDConstants(3, 0, 0);
public static final PIDConstants ROTATIONAL_PID = new PIDConstants(5, 0, 0);
public static final double TIME_TO_FEED = 1.0; // seconds
public static final double TIME_FOR_SCORING = .5; // seconds

public static final Pose2d redAmpSpeakerPose = new Pose2d(
15.59,
6.644,
new Rotation2d(Math.toRadians(120.5))
);
public static final Pose2d blueAmpSpeakerPose = new Pose2d(
0.909,
6.644,
new Rotation2d(Math.toRadians(55.5))
);
public static final Pose2d redSourceSpeakerPose = new Pose2d(
15.636,
4.39,
new Rotation2d(Math.toRadians(-122.5))
);
public static final Pose2d blueSourceSpeakerPose = new Pose2d(
0.864,
4.39,
new Rotation2d(Math.toRadians(-62.5))
);
public static final Pose2d redAmpPose = new Pose2d(
14.7,
7.72,
new Rotation2d(Math.toRadians(-90))
);
public static final Pose2d blueAmpPose = new Pose2d(
1.85,
7.72,
new Rotation2d(Math.toRadians(-90))
);

public static final Pose2d blueSpeakerPose = new Pose2d(
1.45,
5.50,
new Rotation2d(Math.toRadians(0))
);
public static final Pose2d redSpeakerPose = new Pose2d(
15.1,
5.50,
new Rotation2d(Math.toRadians(180))
);

public static final double TRANSLATION_ERROR_MARGIN = 0.4;
public static final double ROTATION_ERROR_MARGIN = 0.4;
}
}
234 changes: 123 additions & 111 deletions src/main/java/frc/robot/subsystems/drive/AutoAlign.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,126 +10,138 @@
import frc.robot.subsystems.manager.ManagerStates;

public class AutoAlign {
private static PIDController xPIDController =
new PIDController(
Constants.AutoAlign.TRANSLATIONAL_PID.kP,
Constants.AutoAlign.TRANSLATIONAL_PID.kI,
Constants.AutoAlign.TRANSLATIONAL_PID.kD);
private static PIDController yPIDController =
new PIDController(
Constants.AutoAlign.TRANSLATIONAL_PID.kP,
Constants.AutoAlign.TRANSLATIONAL_PID.kI,
Constants.AutoAlign.TRANSLATIONAL_PID.kD);
private static final PIDController rotationalPIDController =
new PIDController(
Constants.AutoAlign.ROTATIONAL_PID.kP,
Constants.AutoAlign.ROTATIONAL_PID.kI,
Constants.AutoAlign.ROTATIONAL_PID.kD);
private static Pose2d targetPose2d;
private static AutoAlignInstruction autoAlignInstruction;
private static Timer timer = new Timer();
private static boolean setStateAlready;

private static Drive drive;
private static Manager manager;
private static PIDController xPIDController = new PIDController(
Constants.AutoAlign.TRANSLATIONAL_PID.kP,
Constants.AutoAlign.TRANSLATIONAL_PID.kI,
Constants.AutoAlign.TRANSLATIONAL_PID.kD
);
private static PIDController yPIDController = new PIDController(
Constants.AutoAlign.TRANSLATIONAL_PID.kP,
Constants.AutoAlign.TRANSLATIONAL_PID.kI,
Constants.AutoAlign.TRANSLATIONAL_PID.kD
);
private static final PIDController rotationalPIDController = new PIDController(
Constants.AutoAlign.ROTATIONAL_PID.kP,
Constants.AutoAlign.ROTATIONAL_PID.kI,
Constants.AutoAlign.ROTATIONAL_PID.kD
);
private static Pose2d targetPose2d;
private static AutoAlignInstruction autoAlignInstruction;
private static Timer timer = new Timer();
private static boolean setStateAlready;

public static void setTargetPose(Pose2d target) {
targetPose2d = target;
}
private static Drive drive;
private static Manager manager;

public static void setDrive(Drive inputDrive) {
drive = inputDrive;
}
public static void setTargetPose(Pose2d target) {
targetPose2d = target;
}

public static void setManager(Manager inputManager) {
manager = inputManager;
}
public static void setDrive(Drive inputDrive) {
drive = inputDrive;
}

// calculate chassissSpeed obj
public static void calculateChassisSpeed() {
Pose2d currentPose2d = drive.getPose();
public static void setManager(Manager inputManager) {
manager = inputManager;
}

if (!nearSetPoint()) {
manager.setState(autoAlignInstruction.duringDrive);
// calculate chassissSpeed obj
public static void calculateChassisSpeed() {
Pose2d currentPose2d = drive.getPose();

drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()),
yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()),
rotationalPIDController.calculate(
currentPose2d.getRotation().getRadians(),
targetPose2d.getRotation().getRadians()),
drive.getRotation()));
} else {
// this is the only way i could think of to make the code pretty. please don't murder me
// Amp bar needs time to be able to be fed
if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) {
manager.setState(ManagerStates.IDLE);
drive.setState(DriveStates.REGULAR_DRIVE);
if (!nearSetPoint()) {
manager.setState(autoAlignInstruction.duringDrive);

} else if (autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR
&& timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) {
manager.setState(autoAlignInstruction.atSetPoint);
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
xPIDController.calculate(currentPose2d.getX(), targetPose2d.getX()),
yPIDController.calculate(currentPose2d.getY(), targetPose2d.getY()),
rotationalPIDController.calculate(
currentPose2d.getRotation().getRadians(),
targetPose2d.getRotation().getRadians()
),
drive.getRotation()
)
);
} else {
// this is the only way i could think of to make the code pretty. please don't murder me
// Amp bar needs time to be able to be fed
if (timer.hasElapsed(Constants.AutoAlign.TIME_FOR_SCORING) && setStateAlready) {
manager.setState(ManagerStates.IDLE);
drive.setState(DriveStates.REGULAR_DRIVE);
} else if (
autoAlignInstruction == AutoAlignInstruction.SCORE_AMP_BAR &&
timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)
) {
manager.setState(autoAlignInstruction.atSetPoint);
} else if (
autoAlignInstruction == AutoAlignInstruction.SHOOT &&
timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)
) {
manager.setState(autoAlignInstruction.atSetPoint);
}

} else if (autoAlignInstruction == AutoAlignInstruction.SHOOT
&& timer.hasElapsed(Constants.AutoAlign.TIME_TO_FEED)) {
manager.setState(autoAlignInstruction.atSetPoint);
}
if (!setStateAlready) {
setStateAlready = true;
timer.reset();
timer.start();
}
}
}

if (!setStateAlready) {
setStateAlready = true;
timer.reset();
timer.start();
}
}
}
public static boolean nearSetPoint() {
Pose2d currentPose2d = drive.getPose();

public static boolean nearSetPoint() {
Pose2d currentPose2d = drive.getPose();
if (
Math.abs(currentPose2d.getX() - targetPose2d.getX()) <
Constants.AutoAlign.TRANSLATION_ERROR_MARGIN &&
Math.abs(currentPose2d.getY() - targetPose2d.getY()) <
Constants.AutoAlign.TRANSLATION_ERROR_MARGIN &&
Math.abs(
currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees()
) <
Constants.AutoAlign.ROTATION_ERROR_MARGIN
) {
return true;
}
return false;
}

if (Math.abs(currentPose2d.getX() - targetPose2d.getX())
< Constants.AutoAlign.TRANSLATION_ERROR_MARGIN
&& Math.abs(currentPose2d.getY() - targetPose2d.getY())
< Constants.AutoAlign.TRANSLATION_ERROR_MARGIN
&& Math.abs(
currentPose2d.getRotation().getDegrees() - targetPose2d.getRotation().getDegrees())
< Constants.AutoAlign.ROTATION_ERROR_MARGIN) {
return true;
}
return false;
}

public static void periodic() {
if (Constants.operatorController.getLeftBumper()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redSourceSpeakerPose
: Constants.AutoAlign.blueAmpSpeakerPose);
autoAlignInstruction = AutoAlignInstruction.SHOOT;
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getRightBumper()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redAmpSpeakerPose
: Constants.AutoAlign.blueSourceSpeakerPose);
autoAlignInstruction = AutoAlignInstruction.SHOOT;
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getAButton()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redAmpPose
: Constants.AutoAlign.blueAmpPose);
autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR;
timer.restart();
timer.start();
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getXButton()) {
drive.setState(DriveStates.REGULAR_DRIVE);
}
}
// #TODO add drive to amp bar and auto shoot.
// create timer for feeding and holding
}
public static void periodic() {
if (Constants.operatorController.getLeftBumper()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redSourceSpeakerPose
: Constants.AutoAlign.blueAmpSpeakerPose
);
autoAlignInstruction = AutoAlignInstruction.SHOOT;
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getRightBumper()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redAmpSpeakerPose
: Constants.AutoAlign.blueSourceSpeakerPose
);
autoAlignInstruction = AutoAlignInstruction.SHOOT;
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getAButton()) {
setTargetPose(
(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
? Constants.AutoAlign.redAmpPose
: Constants.AutoAlign.blueAmpPose
);
autoAlignInstruction = AutoAlignInstruction.SCORE_AMP_BAR;
timer.restart();
timer.start();
setStateAlready = false;
drive.setState(DriveStates.AUTO_ALIGN);
} else if (Constants.operatorController.getXButton()) {
drive.setState(DriveStates.REGULAR_DRIVE);
}
}
// #TODO add drive to amp bar and auto shoot.
// create timer for feeding and holding
}
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/subsystems/drive/AutoAlignInstruction.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@
import frc.robot.subsystems.manager.ManagerStates;

public enum AutoAlignInstruction {
SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP),
SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING);
SCORE_AMP_BAR(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP),
SHOOT(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING);

AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) {
this.duringDrive = duringDrive;
this.atSetPoint = atSetPoint;
}
AutoAlignInstruction(ManagerStates duringDrive, ManagerStates atSetPoint) {
this.duringDrive = duringDrive;
this.atSetPoint = atSetPoint;
}

ManagerStates duringDrive;
ManagerStates atSetPoint;
ManagerStates duringDrive;
ManagerStates atSetPoint;
}

0 comments on commit 89acd2d

Please sign in to comment.