Skip to content

Commit

Permalink
Full Pose estimation implemantation
Browse files Browse the repository at this point in the history
  • Loading branch information
@frc1987
frc1987 committed Mar 23, 2024
1 parent 76de783 commit 9661710
Show file tree
Hide file tree
Showing 10 changed files with 174 additions and 174 deletions.
25 changes: 13 additions & 12 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,25 +12,26 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;

public class Robot extends TimedRobot {
private Command m_autonomousCommand;
private Command autoCommand;

private RobotContainer m_robotContainer;
private RobotContainer robotContainer;

@Override
public void robotInit() {
m_robotContainer = new RobotContainer();
m_robotContainer.CANDLES.setAnimationBoth(
robotContainer = new RobotContainer();
robotContainer.CANDLES.setAnimationBoth(
new LarsonAnimation(255, 255, 0, 0, 1.0, 8, BounceMode.Front, 1));
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
robotContainer.updatePoseVision();
}

@Override
public void disabledInit() {
m_robotContainer.CANDLES.setAnimationBoth(new SingleFadeAnimation(255, 0, 0, 0, 0.7, 8));
robotContainer.CANDLES.setAnimationBoth(new SingleFadeAnimation(255, 0, 0, 0, 0.7, 8));
}

@Override
Expand All @@ -42,12 +43,12 @@ public void disabledExit() {}
@Override
public void autonomousInit() {
// m_robotContainer.CANDLES.setAnimationBoth(new SingleFadeAnimation(255, 255, 255, 0, 1.5, 8));
m_robotContainer.CANDLES.setAnimationRight(
robotContainer.CANDLES.setAnimationRight(
new LarsonAnimation(255, 255, 255, 0, 0.1, 8, BounceMode.Front, 1));
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
autoCommand = robotContainer.getAutonomousCommand();

if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
if (autoCommand != null) {
autoCommand.schedule();
}
}

Expand All @@ -59,9 +60,9 @@ public void autonomousExit() {}

@Override
public void teleopInit() {
m_robotContainer.CANDLES.stop();
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
robotContainer.CANDLES.stop();
if (autoCommand != null) {
autoCommand.cancel();
}
}

Expand Down
93 changes: 49 additions & 44 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,11 @@
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.VecBuilder;
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.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
Expand Down Expand Up @@ -61,22 +63,18 @@
import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.Wrist;
import frc.robot.util.Limelight;
import frc.robot.util.Util;

public class RobotContainer {
private final SendableChooser<Command> AUTO_CHOOSER = new SendableChooser<>();
// public final PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev);
public final ShuffleboardTab COMMANDS_TAB = Shuffleboard.getTab("COMMANDS");
public final ShuffleboardTab MATCH_TAB = Shuffleboard.getTab("MATCH");
public final ShuffleboardTab PHOTON_TAB = Shuffleboard.getTab("PHOTON");
public final ShuffleboardTab SHOOTER_TAB = Shuffleboard.getTab("SHOOTER");
public final ShuffleboardTab PROTO_TAB = Shuffleboard.getTab("PROTO");
public final String SPEAKER_LIMELIGHT = "limelight-speaker";
public final Vision INTAKE_PHOTON = new Vision("Arducam_OV9782_USB_Camera", 0.65176, 60);
// public final Vision SPEAKER_PHOTON =
// new Vision("Arducam_OV2311_USB_Camera_1", 0.3, 40, Arrays.asList(4, 7));
// public final Vision AMP_PHOTON =
// new Vision("Arducam_OV2311_USB_Camera", 0.35, 40, Arrays.asList(5, 6));
public final CommandSwerveDrivetrain DRIVETRAIN = DriveConstants.DriveTrain; // My drivetrain
public final Candles CANDLES = new Candles(Constants.LEFT_CANDLE, Constants.RIGHT_CANDLE);
public final Intake INTAKE = new Intake(Constants.INTAKE_TOP_ID, Constants.INTAKE_BOTTOM_ID);
Expand All @@ -91,13 +89,8 @@ public class RobotContainer {
public static boolean isForwardAmpPrimed = false;
public static boolean isReverseAmpPrimed = false;
public static boolean isClimbPrimed = false;
private double MaxSpeed =
DriveConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed
private double MaxAngularRate =
1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity

/* Setting up bindings for necessary control of the swerve drive platform */
private final CommandSwerveDrivetrain drivetrain = DriveConstants.DriveTrain; // My drivetrain
private double MaxSpeed = DriveConstants.kSpeedAt12VoltsMps;
private double MaxAngularRate = 1.5 * Math.PI;

private final SwerveRequest.FieldCentric drive =
new SwerveRequest.FieldCentric()
Expand All @@ -108,15 +101,11 @@ public class RobotContainer {
private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
private final Telemetry logger = new Telemetry(MaxSpeed);

private void configureBindings() {
if (Utils.isSimulation()) {
drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
}
drivetrain.registerTelemetry(logger::telemeterize);
}
private static RobotContainer instance;

public RobotContainer() {
instance = this;
new Util();
configureNamedCommands();
configureShuffleboard();
configureDrivetrain();
Expand All @@ -127,16 +116,6 @@ public RobotContainer() {

private void configureDriverController() {
DRIVER_CONTROLLER.b().onTrue(new ShootSubwoofer(ELEVATOR, WRIST, SHOOTER));
// DRIVER_CONTROLLER
// .y()
// .onTrue(
// new ConditionalCommand(
// new FireFwdAmp(SHOOTER)
// .andThen(new InstantCommand(() -> isForwardAmpPrimed = false)
// .andThen(new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE))),
// new PrepFwdAmp(ELEVATOR, WRIST, SHOOTER)
// .andThen(new InstantCommand(() -> isForwardAmpPrimed = true)),
// () -> isForwardAmpPrimed));

DRIVER_CONTROLLER
.y()
Expand All @@ -161,16 +140,16 @@ private void configureDriverController() {
.andThen(
new AsyncRumble(
DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 700L)));
// DRIVER_CONTROLLER.rightTrigger().onTrue(new FastPointTwice(DRIVETRAIN, SPEAKER_PHOTON));

DRIVER_CONTROLLER
.rightTrigger()
.whileTrue(
new PointAtAprilTag(
DRIVETRAIN,
SPEAKER_LIMELIGHT,
() -> (-DRIVER_CONTROLLER.getLeftY() * Constants.MaxSpeed),
() -> (-DRIVER_CONTROLLER.getLeftX() * Constants.MaxSpeed),
() -> (-DRIVER_CONTROLLER.getRightX() * Constants.MaxSpeed)));
() -> (DRIVER_CONTROLLER.getLeftY()),
() -> (DRIVER_CONTROLLER.getLeftX()),
() -> (DRIVER_CONTROLLER.getRightX())));

DRIVER_CONTROLLER
.rightBumper()
Expand Down Expand Up @@ -215,7 +194,6 @@ private void configureCoDriverController() {
INTAKE.stopTop();
INTAKE.stopCollecting();
})));
// CO_DRIVER_CONTROLLER.leftBumper().onTrue(new FastPointTwice(DRIVETRAIN, SPEAKER_PHOTON));

CO_DRIVER_CONTROLLER.rightTrigger().onTrue(new ShootSubwooferFlat(ELEVATOR, WRIST, SHOOTER));
CO_DRIVER_CONTROLLER
Expand Down Expand Up @@ -278,10 +256,10 @@ private void configureDrivetrain() {
public double getAverageSpeeds() {
double[] speeds =
new double[] {
drivetrain.getModule(0).getDriveMotor().getRotorVelocity().getValueAsDouble(),
drivetrain.getModule(1).getDriveMotor().getRotorVelocity().getValueAsDouble(),
drivetrain.getModule(2).getDriveMotor().getRotorVelocity().getValueAsDouble(),
drivetrain.getModule(3).getDriveMotor().getRotorVelocity().getValueAsDouble()
DRIVETRAIN.getModule(0).getDriveMotor().getRotorVelocity().getValueAsDouble(),
DRIVETRAIN.getModule(1).getDriveMotor().getRotorVelocity().getValueAsDouble(),
DRIVETRAIN.getModule(2).getDriveMotor().getRotorVelocity().getValueAsDouble(),
DRIVETRAIN.getModule(3).getDriveMotor().getRotorVelocity().getValueAsDouble()
};
double total = 0.0;
double avg = 0.0;
Expand Down Expand Up @@ -325,16 +303,15 @@ public void configureShuffleboard() {
SHOOTER.stopFeeder();
SHOOTER.stopShooter();
})));
PHOTON_TAB.addDouble(
"DISTANCE_TO_SPEAKER", () -> Util.getInterpolatedDistance(SPEAKER_LIMELIGHT));
PHOTON_TAB.addDouble("DISTANCE_TO_SPEAKER", () -> Util.getDistance(SPEAKER_LIMELIGHT));

COMMANDS_TAB.add(
"Coast Swerve",
new InstantCommand(
() -> {
for (int i = 0; i < 4; i++) {
drivetrain.getModule(i).getDriveMotor().setNeutralMode(NeutralModeValue.Coast);
drivetrain.getModule(i).getSteerMotor().setNeutralMode(NeutralModeValue.Coast);
DRIVETRAIN.getModule(i).getDriveMotor().setNeutralMode(NeutralModeValue.Coast);
DRIVETRAIN.getModule(i).getSteerMotor().setNeutralMode(NeutralModeValue.Coast);
}
})
.ignoringDisable(true));
Expand All @@ -344,8 +321,8 @@ public void configureShuffleboard() {
new InstantCommand(
() -> {
for (int i = 0; i < 3; i++) {
drivetrain.getModule(i).getDriveMotor().setNeutralMode(NeutralModeValue.Brake);
drivetrain.getModule(i).getSteerMotor().setNeutralMode(NeutralModeValue.Brake);
DRIVETRAIN.getModule(i).getDriveMotor().setNeutralMode(NeutralModeValue.Brake);
DRIVETRAIN.getModule(i).getSteerMotor().setNeutralMode(NeutralModeValue.Brake);
}
})
.ignoringDisable(true));
Expand Down Expand Up @@ -479,4 +456,32 @@ public void addAuto(String autoName) {
public Command getAutonomousCommand() {
return AUTO_CHOOSER.getSelected();
}

public Pose2d getPose() {
return DRIVETRAIN.getPose();
}

public void updatePoseVision() {
Limelight.PoseEstimate pose = Limelight.getBotPoseEstimate_wpiBlue(SPEAKER_LIMELIGHT);
if (pose.tagCount >= 2) {
DriverStation.reportWarning("ADDING POSE BASED ON 2 TAGS", false);
DRIVETRAIN.addVisionMeasurement(
pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999));
} else {
if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) {
DriverStation.reportWarning(
"ADDING POSE BASED ON AMBIGUITY OF "
+ pose.rawFiducials[0].ambiguity
+ " ON TAG "
+ pose.rawFiducials[0].id,
false);
DRIVETRAIN.addVisionMeasurement(
pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999));
}
}
}

public static RobotContainer get() {
return instance;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/commands/control/note/ShootNote.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ public ShootNote(Shooter shooter, Elevator elevator, double shootRPM) {
lineBreakDebouncer.calculate(!shooter.isCenterBroken())), // probably debounce this
new InstantCommand(shooter::stopFeeder, shooter),
new WaitUntilCommand(() -> lineBreakDebouncer.calculate(shooter.isCenterBroken())),
new WaitCommand(0.1),
new InstantCommand(shooter::stopShooter, shooter));
}
}
37 changes: 22 additions & 15 deletions src/main/java/frc/robot/commands/movement/PointAtAprilTag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
import frc.robot.constants.DriveConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.util.Limelight;
import frc.robot.util.Limelight.RawFiducial;
import frc.robot.util.Util;
import java.util.function.DoubleSupplier;

Expand All @@ -33,6 +35,7 @@ public class PointAtAprilTag extends Command {
private DoubleSupplier velocityYSupplier = () -> 0.0;
private DoubleSupplier rotationSupplier = () -> 0.0;
private DoubleSupplier rotationSupplier2 = () -> 0.0;
private double kP = 0.09;

double rotationRate = 0;

Expand Down Expand Up @@ -76,19 +79,22 @@ public double getRotationRate() {
@Override
public void execute() {

double xOffset = 0.0;
// System.out.println("Starting Execute");

double xOffset = Limelight.getTX(speakerLimelight);
for (RawFiducial fiducial :
Limelight.getBotPoseEstimate_wpiBlue(speakerLimelight).rawFiducials) {
if (fiducial.id == 4 || fiducial.id == 13) {
xOffset = fiducial.txnc;
}
}

// TODO: changeme please :)

// if (xOffset > 1) {
// rotationRate = kP * xOffset;
// } else {

// }

rotationRate = xOffset * 0.09;
if (xOffset < 0.7) {
rotationRate = 0.12 * xOffset;
} else {
rotationRate = kP * xOffset;
}

System.out.println("Attempted RotationRate: " + rotationRate);
// rotationRate = Math.copySign(MathUtil.clamp(Math.abs(rotationRate), 0, 2.75), rotationRate);
Expand All @@ -97,26 +103,27 @@ public void execute() {

// Degrees within acceptance

double acceptableError = 0.3;
double acceptableError = 0.05;
if (Math.abs(rotationRate) < acceptableError) {
rotationRate = 0;
}

System.out.println(rotationRate);

if (!Util.canSeeTarget(speakerLimelight)) {
rotationRate = rotationSupplier.getAsDouble();
rotationRate = Util.squareValue(rotationSupplier.getAsDouble()) * Math.PI * 3.5;
DriverStation.reportWarning("No Tag found in PointAtAprilTag", false);
}

drivetrain.setControl(
drive
.withVelocityX(
translationYSlewRate.calculate(
-velocityYSupplier.getAsDouble())) // Drive forward with
Util.squareValue(-velocityXSupplier.getAsDouble())
* DriveConstants.kSpeedAt12VoltsMps) // Drive forward with
// negative Y (forward)
.withVelocityY(
translationXSlewRate.calculate(
-velocityXSupplier.getAsDouble())) // Drive left with negative X (left)
Util.squareValue(-velocityYSupplier.getAsDouble())
* DriveConstants.kSpeedAt12VoltsMps) // Drive left with negative X (left)
.withRotationalRate(-rotationRate) // Drive counterclockwise with negative X (left)
);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/movement/SwerveCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ public void execute() {
Util.squareValue(-TRANSLATION_X_SUPPLIER.getAsDouble()) * DriveConstants.kSpeedAt12VoltsMps;
double yPercentage =
Util.squareValue(-TRANSLATION_Y_SUPPLIER.getAsDouble()) * DriveConstants.kSpeedAt12VoltsMps;
double rotationPercentage = Util.squareValue(-ROTATION_SUPPLIER.getAsDouble() * Math.PI * 3.5);
double rotationPercentage = Util.squareValue(-ROTATION_SUPPLIER.getAsDouble()) * Math.PI * 3.5;

if (isCardinalLocking && !Util.isWithinTolerance(ROTATION_SUPPLIER.getAsDouble(), 0.0, 0.25)) {
isCardinalLocking = false;
Expand Down
Loading

0 comments on commit 9661710

Please sign in to comment.