Skip to content

Commit

Permalink
Merge branch 'main' into autoRoutines
Browse files Browse the repository at this point in the history
  • Loading branch information
rutmanz authored Feb 11, 2024
2 parents 8b42b4e + 1d4aac8 commit 0c75127
Show file tree
Hide file tree
Showing 5 changed files with 112 additions and 32 deletions.
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 @@ -235,6 +236,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
3 changes: 3 additions & 0 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
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;
Expand Down Expand Up @@ -220,6 +222,7 @@ private void configureButtonBindings() {
indexer.setDefaultCommand(new IntakeCommand(indexer, tramp));

driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
driver.y().toggleOnTrue(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())) {
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

0 comments on commit 0c75127

Please sign in to comment.