Skip to content
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.

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

Speaker targeting #32

Merged
merged 7 commits into from
Feb 11, 2024
Merged
Show file tree
Hide file tree
Changes from 6 commits
Commits
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
11 changes: 11 additions & 0 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package org.team1540.robot2024;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
Expand Down Expand Up @@ -232,6 +233,16 @@ public static class Tramp {
public static final int MOTOR_ID = -1; //TODO: Configure this later
}

public static class Targeting {
// TODO: tune these
public static final double ROT_KP = 1.18;
public static final double ROT_KI = 0.0;
public static final double ROT_KD = 0.0;

public static final Pose2d SPEAKER_POSE =
new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d());
}

public static boolean isTuningMode() {
return tuningMode && !DriverStation.isFMSAttached();
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,14 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.DriveWithSpeakerTargetingCommand;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.commands.shooter.ShootSequence;
import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.elevator.Elevator;
import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
Expand Down Expand Up @@ -176,6 +175,7 @@ private void configureButtonBindings() {
indexer.setDefaultCommand(new IntakeCommand(indexer, tramp));

driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
driver.y().toggleOnFalse(new DriveWithSpeakerTargetingCommand(drivetrain, driver));
driver.b().onTrue(
Commands.runOnce(
() -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())),
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
package org.team1540.robot2024.commands;

import com.pathplanner.lib.util.GeometryUtil;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.littletonrobotics.junction.Logger;
import org.team1540.robot2024.subsystems.drive.Drivetrain;
import org.team1540.robot2024.util.LoggedTunableNumber;

import static org.team1540.robot2024.Constants.Targeting.*;

public class DriveWithSpeakerTargetingCommand extends Command {
private final Drivetrain drivetrain;
private final CommandXboxController controller;

private final SlewRateLimiter xLimiter = new SlewRateLimiter(2);
private final SlewRateLimiter yLimiter = new SlewRateLimiter(2);
private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD);

private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP);
private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI);
private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD);

private boolean isFlipped;
private Pose2d speakerPose;

public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) {
this.drivetrain = drivetrain;
this.controller = controller;
rotController.enableContinuousInput(-Math.PI, Math.PI);
addRequirements(drivetrain);
}

@Override
public void initialize() {
rotController.reset();

isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE;
}

@Override
public void execute() {
if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

tuning mode?

rotController.setPID(kP.get(), kI.get(), kD.get());
}

Rotation2d targetRot =
drivetrain.getPose().minus(speakerPose).getTranslation().getAngle()
.rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180));
Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot));
Logger.recordOutput("Targeting/speakerPose", speakerPose);

double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1);
double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1);
double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians());
drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped);
}

@Override
public void end(boolean interrupted) {
drivetrain.stop();
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,6 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
Expand All @@ -30,41 +25,18 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle

@Override
public void initialize() {
xLimiter.reset(0);
yLimiter.reset(0);
rotLimiter.reset(0);
isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
}

@Override
public void execute() {
double xPercent = xLimiter.calculate(-controller.getLeftY());
double yPercent = yLimiter.calculate(-controller.getLeftX());
double rotPercent = rotLimiter.calculate(-controller.getRightX());
double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1);
double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1);
double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1);

// Apply deadband
double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1);
Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
double omega = MathUtil.applyDeadband(rotPercent, 0.1);

// Calculate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();

// Convert to field relative speeds & send command
drivetrain.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(),
omega * drivetrain.getMaxAngularSpeedRadPerSec(),
isFlipped
? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180))
: drivetrain.getRotation()
)
);
drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
Expand Down Expand Up @@ -125,6 +126,27 @@ public void runVelocity(ChassisSpeeds speeds) {
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
}

public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean isFlipped) {
Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
double linearMagnitude = Math.hypot(xPercent, yPercent);

Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();

// Convert to field relative
runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
rotPercent * getMaxAngularSpeedRadPerSec(),
isFlipped
? getRotation().plus(Rotation2d.fromDegrees(180))
: getRotation()
)
);
}

/**
* Stops the drive.
*/
Expand Down
Loading