Skip to content

Commit

Permalink
Merge pull request #96 from DurhamAcademy/71-aimbot-shooter-command
Browse files Browse the repository at this point in the history 
aimbot shooter command
  • Loading branch information
@a1cd
a1cd authored Mar 1, 2024
2 parents 611504b + dad0ca1 commit d341f35
Show file tree
Hide file tree
Showing 8 changed files with 255 additions and 63 deletions.
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,8 @@ public void robotInit() {
switch (Constants.currentMode) {
case REAL:
// Running on a real robot, log to a USB stick ("/U/logs")
Logger.addDataReceiver(new WPILOGWriter());
// todo: check for file path
// Logger.addDataReceiver(new WPILOGWriter());
Logger.addDataReceiver(new NT4Publisher());
break;

Expand Down
22 changes: 12 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,13 +180,7 @@ private void configureButtonBindings() {
() -> -driverController.getLeftY(),
() -> -driverController.getLeftX(),
() -> -driverController.getRightX()));
intake.setDefaultCommand(
new RunCommand(
() -> {
intake.setIntakePosition(Rotation2d.fromDegrees(-90));
intake.setRollerPercentage(0.0);
},
intake));
intake.setDefaultCommand(IntakeCommands.idleCommand(intake));
feeder.setDefaultCommand(new RunCommand(() -> feeder.runVolts(0.0), feeder));
shooter.setDefaultCommand(new RunCommand(() -> {
shooter.shooterRunVolts(0.0);
Expand All @@ -205,6 +199,14 @@ private void configureButtonBindings() {
// ---- DRIVETRAIN COMMANDS ----
driverController.x().whileTrue(Commands.runOnce(drive::stopWithX, drive));

var command =
DriveCommands.aimAtSpeakerCommand(
drive,
() -> -driverController.getLeftY(),
() -> -driverController.getLeftX(),
() -> -driverController.getRightX());
driverController.a().onTrue(command.getCommand());

// ---- INTAKE COMMANDS ----
driverController
.leftTrigger()
Expand Down Expand Up @@ -273,11 +275,11 @@ private void configureButtonBindings() {
driverController
.rightTrigger()
.whileTrue(
new RunCommand(() -> shooter.setTargetShooterAngle(new Rotation2d(-0.61)))
new RunCommand(() -> shooter.setTargetShooterAngle(new Rotation2d(-0.61)))
.andThen(
(new RunCommand(
() ->
shooter.shooterRunVelocity(5000), //THIS NUMBER NEEDS TO BE CALIBRATED
() -> shooter.shooterRunVelocity(5000) , //THIS NUMBER NEEDS TO BE CALIBRATED

intake))));
break;
case Shooter:
Expand Down
260 changes: 217 additions & 43 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,64 +13,238 @@

package frc.robot.commands;

import static edu.wpi.first.units.Units.*;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.controller.ProfiledPIDController;
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.trajectory.TrapezoidProfile;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.subsystems.drive.Drive;
import org.littletonrobotics.junction.Logger;

import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;

import static edu.wpi.first.math.MathUtil.applyDeadband;
import static edu.wpi.first.math.MathUtil.inputModulus;
import static edu.wpi.first.units.Units.*;

public class DriveCommands {

private static final double DEADBAND = 0.1;
private static final double DEADBANDX = 1.0;
private static final double DEADBANDX = 1.0;
private static final double CANCEL_COMMAND_DEADBAND = 0.2;
private static final double DRIVE_ROTATION_P_VALUE = 35.0;
private static TrapezoidProfile.Constraints rotationConstraints =
new TrapezoidProfile.Constraints(
RadiansPerSecond.of(5), RadiansPerSecond.per(Second).of(5.0));

private DriveCommands() {}
private DriveCommands() {
}

/**
* Field relative drive command using two joysticks (controlling linear and angular velocities).
/*
-------------------
---- UTILITIES ----
-------------------
*/
public static Command joystickDrive( //make shooter command class with an auto aim method
Drive drive,
DoubleSupplier xSupplier,
DoubleSupplier ySupplier,
DoubleSupplier omegaSupplier) {
return Commands.run(
() -> {
// Apply deadband
double linearMagnitude =
MathUtil.applyDeadband(
Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
Rotation2d linearDirection =
new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);

// Square values
linearMagnitude = linearMagnitude * linearMagnitude;
omega = Math.copySign(omega * omega, omega);

// Calcaulate 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
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
drive.getRotation()));

},
drive);
}

public static Translation2d getLinearVelocity(
DoubleSupplier xSupplier, DoubleSupplier ySupplier) {
// Apply deadband
double linearMagnitude =
applyDeadband(Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());

// Square values
linearMagnitude = linearMagnitude * linearMagnitude;

// Calcaulate new linear velocity
return new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
.getTranslation();
}

/*
------------------
---- COMMANDS ----
------------------
*/

/**
* Field relative drive command using two joysticks (controlling linear and angular velocities).
*/
public static Command joystickDrive( //make shooter command class with an auto aim method
Drive drive,
DoubleSupplier xSupplier,
DoubleSupplier ySupplier,
DoubleSupplier omegaSupplier) {
return Commands.run(
() -> {
// Apply deadband
double linearMagnitude =
MathUtil.applyDeadband(
Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
Rotation2d linearDirection =
new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);

// Square values
linearMagnitude = linearMagnitude * linearMagnitude;
omega = Math.copySign(omega * omega, omega);

// Calcaulate 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
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
drive.getRotation()));

},
drive);
}

public static class CommandAndReadySupplier {
private Command command;
private BooleanSupplier readySupplier;

private CommandAndReadySupplier(Command command, BooleanSupplier readySupplier) {
this.command = command;
this.readySupplier = readySupplier;
}

public Command getCommand() {
return command;
}

public BooleanSupplier getReadySupplier() {
return readySupplier;
}
}

public static CommandAndReadySupplier aimAtSpeakerCommand(
Drive drive,
DoubleSupplier xSupplier,
DoubleSupplier ySupplier,
DoubleSupplier omegaSupplier) {

final Pose2d[] previousPose = {null};
ProfiledPIDController rotationController =
new ProfiledPIDController(.5, 0, .0, new TrapezoidProfile.Constraints(1, 2));

rotationController.enableContinuousInput(Rotations.toBaseUnits(0), Rotations.toBaseUnits(1));

var command =
new RunCommand(
() -> {
// rotationController.setP(SmartDashboard.getNumber("rotationPidP", 0.0));
// rotationController.setI(SmartDashboard.getNumber("rotationPidI", 0.0));
// rotationController.setD(SmartDashboard.getNumber("rotationPidD", 0.0));

// Calcaulate new linear velocity
Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier);
// Get the angle to point at the goal
var goalAngle =
ShooterCommands.getSpeakerPos().toPose2d()
.getTranslation()
.minus(drive.getPose().getTranslation())
.getAngle();
Transform2d robotVelocity;
Pose2d movingWhileShootingTarget;
if (previousPose[0] != null) {
robotVelocity = previousPose[0].minus(drive.getPose());

double distance =
ShooterCommands.getSpeakerPos().toPose2d()
.getTranslation()
.getDistance(previousPose[0].getTranslation());
if (distance != 0) {
movingWhileShootingTarget =
ShooterCommands.getSpeakerPos().toPose2d().plus(
robotVelocity.times(0.02).times(16.5 / distance));
} else movingWhileShootingTarget = ShooterCommands.getSpeakerPos().toPose2d();
} else movingWhileShootingTarget = ShooterCommands.getSpeakerPos().toPose2d();
Logger.recordOutput("speakerAimTargetPose", movingWhileShootingTarget);
/*
|--------|
|-------|
|------|
|----|
|---|
|--|
|-|*
|=>
*/


Measure<Velocity<Angle>> goalAngleVelocity = null;
if (previousPose[0] != null) {
var previousAngle =
movingWhileShootingTarget
.getTranslation()
.minus(previousPose[0].getTranslation())
.getAngle();
var currentAngle = goalAngle;
goalAngleVelocity =
Radians.of(currentAngle.minus(previousAngle).getRadians())
.per(Seconds.of(0.02));
} else goalAngleVelocity = RadiansPerSecond.zero();
Logger.recordOutput("goalAngleVelocity", goalAngleVelocity);
// calculate how much speed is needed to get ther
// rotationController.reset(
// new TrapezoidProfile.State(
// Radians.of(drive.getRotation().getRadians()),
// drive.getAnglularVelocity()));
// rotationController.setGoal(
// new TrapezoidProfile.State(
// Radians.of(goalAngle.getRadians())));
var value = rotationController.calculate(
inputModulus(drive.getPose().getRotation().getRotations(), 0, 1),
// new TrapezoidProfile.State(
inputModulus(goalAngle.getRotations(), 0, 1)
// goalAngleVelocity.in(RotationsPerSecond)*.0025
// )
);
Logger.recordOutput("angle value", (value));
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
(value) * -6.28,
drive.getRotation()));
previousPose[0] = drive.getPose();
}, drive)
.beforeStarting(() -> {
rotationController.reset(MathUtil.inputModulus(drive.getRotation().getRotations(), 0, 1));
}, drive)
.until(
() -> {
// if the controller is giving a turn input, end the command
// because the driver is trying to take back control
var isGTE = omegaSupplier.getAsDouble() >= CANCEL_COMMAND_DEADBAND;
var isLTE = omegaSupplier.getAsDouble() <= -CANCEL_COMMAND_DEADBAND;
return isLTE || isGTE;
});
return new CommandAndReadySupplier(command, () -> rotationController.atGoal());
}

public static CommandAndReadySupplier aimAtSpeakerCommand(
Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) {
return aimAtSpeakerCommand(drive, xSupplier, ySupplier, () -> 0.0);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/IntakeCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ public static Command flushIntake(Intake intake){
public static Command idleCommand(Intake intake) {
return new RunCommand(
() -> {
intake.setIntakePosition(Rotation2d.fromDegrees(-110.0));
intake.setIntakePosition(Rotation2d.fromRadians(-2.1));
intake.setRollerPercentage(0.0);
},
intake);
Expand Down
16 changes: 11 additions & 5 deletions src/main/java/frc/robot/commands/ShooterCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.drive.Drive;
Expand All @@ -11,11 +12,18 @@

import java.util.function.DoubleSupplier;

import static edu.wpi.first.wpilibj.DriverStation.Alliance.Blue;


public class ShooterCommands {
static InterpolatingDoubleTreeMap distanceToAngle = new InterpolatingDoubleTreeMap();
static InterpolatingDoubleTreeMap distanceToRPM = new InterpolatingDoubleTreeMap();
static Pose3d speakerPos = new Pose3d(0.24, 5.50, 2.13, new Rotation3d());

public static Pose3d getSpeakerPos() {
return (DriverStation.getAlliance().orElse(Blue).equals(Blue)) ?
new Pose3d(0.24, 5.50, 2.13, new Rotation3d()) :
new Pose3d(16.27, 5.50, 2.13, new Rotation3d());
}
static Transform3d shooterOffset = new Transform3d(new Translation3d(0.0, 0.239, .669), new Rotation3d());

private static double getDistance(Pose3d pose3d) {
Expand All @@ -38,15 +46,13 @@ public static Command autoAim(Shooter shooter, Drive drive, DoubleSupplier suppl
//the parameter is the robot, idk how to declare it, also this returns the angle
return Commands.run(() -> {
Pose3d shooter1 = new Pose3d(drive.getPose()).plus(shooterOffset);
Pose3d pose3d = speakerPos.relativeTo(shooter1);
Pose3d pose3d = getSpeakerPos().relativeTo(shooter1);
double distance = getDistance(pose3d);
Logger.recordOutput("distanceFromGoal", distance);
double atan = Math.atan(pose3d.getZ() / distance);
shooter.setTargetShooterAngle(Rotation2d.fromRadians(distanceToAngle.get(distance)));
shooter.shooterRunVelocity(distanceToRPM.get(distance));
}, shooter).handleInterrupt(() -> {
shooter.shooterRunVelocity(0.0);
});
}, shooter).handleInterrupt(() -> shooter.shooterRunVelocity(0.0));
}

private static class Result {
Expand Down
Loading

0 comments on commit d341f35

Please sign in to comment.