private Command auto = new DoNothing();
// Controllers are defined here
- private BaseDriverConfig driver = null;
+ private PS5ControllerDriverConfig driver = null;
private Operator operator = null;
/**
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Rectangle2d;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.Robot;
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.controls.BaseDriverConfig;
+import frc.robot.controls.PS5ControllerDriverConfig;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.TrenchAssist.TrenchAssist;
+import frc.robot.util.TrenchAssist.TrenchAssist2;
import frc.robot.util.TrenchAssist.TrenchAssistConstants;
import frc.robot.util.Vision.DriverAssist;
+import lib.controllers.PS5Controller.PS5Axis;
/**
* Default drive command. Drives robot using driver controls.
*/
public class DefaultDriveCommand extends Command {
protected final Drivetrain swerve;
- protected final BaseDriverConfig driver;
+ protected final PS5ControllerDriverConfig driver;
public DefaultDriveCommand(
Drivetrain swerve,
- BaseDriverConfig driver) {
+ PS5ControllerDriverConfig driver) {
this.swerve = swerve;
this.driver = driver;
double forwardTranslation = driver.getForwardTranslation();
double sideTranslation = driver.getSideTranslation();
double rotation = -driver.getRotation();
+ // if (swerve.getTrenchAssist()) {
+ // sideTranslation = Math
+ // .sqrt(driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y)
+ // * driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y))
+ // *
+ // new Rotation2d(driver.controller.get(PS5Axis.LEFT_X), driver.controller.get(PS5Axis.LEFT_Y))
+ // .rotateBy(swerve.getYaw()).getCos();
+ // forwardTranslation = Math
+ // .sqrt(driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y)
+ // * driver.controller.get(PS5Axis.LEFT_X) * driver.controller.get(PS5Axis.LEFT_Y))
+ // *
+ // new Rotation2d(driver.controller.get(PS5Axis.LEFT_X), driver.controller.get(PS5Axis.LEFT_Y))
+ // .rotateBy(swerve.getYaw()).getSin();
+ // }
double slowFactor = driver.getIsSlowMode() ? DriveConstants.SLOW_DRIVE_FACTOR : 1;
Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
if (swerve.getTrenchAssist()) {
- drive(TrenchAssist.calculate(swerve, corrected));
+ drive(TrenchAssist2.calculate(swerve, corrected));
} else {
drive(corrected);
}
import frc.robot.subsystems.turret.Turret;
import frc.robot.util.FieldZone;
import frc.robot.util.ShootingTarget;
-import frc.robot.util.Vision.TurretVision;
public class SimpleAutoShoot extends Command {
private Turret turret;
private Drivetrain drivetrain;
- private TurretVision turretVision;
private Shooter shooter;
private double fieldAngleRad;
// System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
}
- public void adjustWithTurretCam() {
- if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){
- // Adjust turret setpoint based on vision input
- if(Robot.getAlliance() == Alliance.Blue){
- yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get());
- }
- else{
- yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get());
- }
- double error = yawToTagCamera - yawToTag;
- adjustedSetpoint = turretSetpoint + error;
- }
- }
public void updateYawToTag(){
// Calculate the yaw offset to the tag
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.constants.VisionConstants;
import frc.robot.controls.BaseDriverConfig;
+import frc.robot.controls.PS5ControllerDriverConfig;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.util.Vision.DetectedObject;
private static DetectedObject cachedObject;
- public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier<DetectedObject> objectSupplier){
+ public AimAtGamePiece(Drivetrain drive, PS5ControllerDriverConfig driver, Supplier<DetectedObject> objectSupplier){
super(drive, driver);
this.objectSupplier = objectSupplier;
}
* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
+ public final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
private final BooleanSupplier slowModeSupplier = () -> false;
private boolean intakeBoolean = true;
private Command autoShoot = null;
public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
- driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
getDrivetrain().setIsAlign(false);
getDrivetrain().setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
}));
// Align wheels
- driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
+ controller.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
() -> getDrivetrain().setStateDeadband(false),
getDrivetrain()::alignWheels,
interrupted -> getDrivetrain().setStateDeadband(true),
() -> false, getDrivetrain()).withTimeout(2));
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
+ controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
.onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
- driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
+ controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
.onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false)));
@Override
public double getRawSideTranslation() {
- return driver.get(PS5Axis.LEFT_X);
+ return controller.get(PS5Axis.LEFT_X);
}
@Override
public double getRawForwardTranslation() {
- return driver.get(PS5Axis.LEFT_Y);
+ return controller.get(PS5Axis.LEFT_Y);
}
@Override
public double getRawRotation() {
- return driver.get(PS5Axis.RIGHT_X);
+ return controller.get(PS5Axis.RIGHT_X);
}
@Override
public double getRawHeadingAngle() {
- return Math.atan2(driver.get(PS5Axis.RIGHT_X), -driver.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
}
@Override
public double getRawHeadingMagnitude() {
- return Math.hypot(driver.get(PS5Axis.RIGHT_X), driver.get(PS5Axis.RIGHT_Y));
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
}
@Override
}
public void startRumble() {
- driver.rumbleOn();
+ controller.rumbleOn();
}
public void endRumble() {
- driver.rumbleOff();
+ controller.rumbleOff();
}
}
return Optional.empty();
}
- static Translation2d calculateCorrection(Rectangle2d[] rectangles, Drivetrain drive, ChassisSpeeds predictedSpeeds) {
- Pose2d pose = drive.getPose();
-
- Translation2d velocityRobotRelative = new Translation2d(predictedSpeeds.vxMetersPerSecond,
- predictedSpeeds.vyMetersPerSecond);
-
- Translation2d velocityFieldRelative = velocityRobotRelative.rotateBy(drive.getYaw());
-
- double halfRobotWidth = 0.5 * DriveConstants.ROBOT_WIDTH_WITH_BUMPERS;
-
- Translation2d[] robotCorners = new Translation2d[] {
- pose.transformBy(new Transform2d(new Translation2d(halfRobotWidth, 0), new Rotation2d(0.0)))
- .getTranslation(),
- pose.transformBy(new Transform2d(new Translation2d(0, halfRobotWidth), new Rotation2d(0.0)))
- .getTranslation(),
- pose.transformBy(new Transform2d(new Translation2d(-halfRobotWidth, 0), new Rotation2d(0.0)))
- .getTranslation(),
- pose.transformBy(new Transform2d(new Translation2d(0, -halfRobotWidth), new Rotation2d(0.0)))
- .getTranslation(),
- };
-
- for (Translation2d corner : robotCorners) {
- for (Rectangle2d rectangle : rectangles) {
- Optional<Double> distanceOptional = rayCast(rectangle, corner, velocityFieldRelative, 1.0);
- if (distanceOptional.isPresent()) {
- double timeToCollision = distanceOptional.get(); // seconds
- double timeWindow = 1.0; //secs
- double scale = Math.max(0.0, 1.0 - (timeToCollision / timeWindow));
-
-
- if (drive.getPose().getY() > rectangle.getCenter().getY() + (rectangle.getYWidth() / 2)) {
- // above rectangle: push +90 deg
- return velocityFieldRelative.rotateBy(new Rotation2d(Units.degreesToRadians(90))).times(scale);
- } else if (drive.getPose().getY() <= rectangle.getCenter().getY() - (rectangle.getYWidth() / 2)) {
- // below rectangle push -90 deg
- return velocityFieldRelative.rotateBy(new Rotation2d(Units.degreesToRadians(-90))).times(scale);
- }
-
- // emergency fallback
- if (rayCast(rectangle, corner, velocityFieldRelative, 0.2).isPresent()) {
- return velocityFieldRelative.unaryMinus();
- }
- }
- }
- }
- return new Translation2d(0, 0);
- }
+ public static ChassisSpeeds funky(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+ ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw());
+ ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(speedsFieldRelative.vxMetersPerSecond, 0.0, speedsFieldRelative.omegaRadiansPerSecond);
+ Logger.recordOutput("slideTranslation", new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond));
- public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
- Translation2d correctionFieldRelative = calculateCorrection(TrenchAssistConstants.OBSTACLES, drive, chassisSpeeds);
-
- Logger.recordOutput("TrenchCorrectionFieldRelative", correctionFieldRelative);
-
- ChassisSpeeds correctionRobot = convertToChassisSpeedsRobotRelative(correctionFieldRelative, drive);
-
- double vx = chassisSpeeds.vxMetersPerSecond + correctionRobot.vxMetersPerSecond;
- double vy = chassisSpeeds.vyMetersPerSecond + correctionRobot.vyMetersPerSecond;
+ var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
+ var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw().unaryMinus());
+ return new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond);
+ }
- return new ChassisSpeeds(vx, vy, chassisSpeeds.omegaRadiansPerSecond);
+ public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
+ double yawRadians = drive.getYaw().unaryMinus().getRadians();
+
+ double fieldRelativeX = chassisSpeeds.vxMetersPerSecond * Math.cos(yawRadians) - chassisSpeeds.vyMetersPerSecond * Math.sin(yawRadians);
+ double fieldRelativeY = chassisSpeeds.vxMetersPerSecond * Math.sin(yawRadians) + chassisSpeeds.vyMetersPerSecond * Math.cos(yawRadians);
+
+ fieldRelativeY = 0;
+
+ double neutralizedRobotX = fieldRelativeX * Math.cos(yawRadians) + fieldRelativeY * Math.sin(yawRadians);
+ double neutralizedRobotY = -fieldRelativeX * Math.sin(yawRadians) + fieldRelativeY * Math.cos(yawRadians);
+
+ var x = new Translation2d(neutralizedRobotX, neutralizedRobotY).rotateBy(drive.getYaw().unaryMinus());
+
+ return new ChassisSpeeds(x.getX(), x.getY(), chassisSpeeds.omegaRadiansPerSecond);
}
public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){
public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive){
Rotation2d yaw = drive.getYaw();
Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
- return robotTranslation.rotateBy(yaw.times(-1));
+ return robotTranslation.rotateBy(yaw.times(-1));
}
}
\ No newline at end of file
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
public class TrenchAssistConstants {
public static final Rectangle2d[] OBSTACLES = new Rectangle2d[]{
// new Rectangle2d(new Translation2d(99.99, 99.9), new Translation2d(0.0, 0.0)),
};
+ public static final double[] SLIDE_ZONES = new double[]{
+ 8.07 - Units.inchesToMeters(25.0),
+ Units.inchesToMeters(25.0),
+ };
}
\ No newline at end of file
+++ /dev/null
-package frc.robot.util.Vision;
-
-import java.util.Optional;
-
-import org.photonvision.PhotonCamera;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.math.util.Units;
-
-public class TurretVision {
-
- private final PhotonCamera camera;
-
- public TurretVision(String cameraName) {
- camera = new PhotonCamera(cameraName);
- }
-
- /**
- * @param tagId AprilTag ID to aim at
- * @return Optional yaw error in radians (positive = target to the right, negative = target to the left)
- */
- public Optional<Double> getYawToTagRad(int tagId) {
- PhotonPipelineResult result = camera.getLatestResult();
-
- if (!result.hasTargets()) {
- return Optional.empty();
- }
-
- for (PhotonTrackedTarget target : result.getTargets()) {
- if (target.getFiducialId() == tagId) {
- // PhotonVision yaw is in DEGREES
- double yawRad = Units.degreesToRadians(target.getYaw());
- return Optional.of(yawRad);
- }
- }
-
- return Optional.empty();
- }
-
- public boolean canSeeTag(int tagId) {
- PhotonPipelineResult result = camera.getLatestResult();
- if (!result.hasTargets()) return false;
-
- for (PhotonTrackedTarget target : result.getTargets()) {
- if (target.getFiducialId() == tagId) {
- return true;
- }
- }
- return false;
- }
-}