Skip to content

Commit

Permalink
New Amp values, passing and shooting while moving improvements, plus …
Browse files Browse the repository at this point in the history 
…some refactoring
  • Loading branch information
@danjburke12
danjburke12 committed Apr 10, 2024
1 parent 048ef6b commit 57b88d0
Show file tree
Hide file tree
Showing 13 changed files with 224 additions and 61 deletions.
13 changes: 7 additions & 6 deletions src/main/java/com/team1701/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -433,10 +433,10 @@ public static final class Shooter {
new LoggedTunableNumber("Shooter/Rotation/TunableAngleRadians", 1.0);

public static final LoggedTunableNumber kLowerAmpSpeed =
new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 70);
new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 75);

public static final LoggedTunableNumber kUpperAmpSpeed =
new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 70);
new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 75);

public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp");
public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd");
Expand Down Expand Up @@ -495,10 +495,11 @@ public static final class Shooter {
};

public static final double[][] kShooterForwardVelocityToHeadingOffset = {
{66, 13},
{73, 9},
{89, 2},
{100, 1}
{315 * Math.cos(.975), 9},
{370 * Math.cos(.689), 6},
{437.2 * Math.cos(.483), 4},
{493 * Math.cos(.4), 2},
{560 * Math.cos(.306), 0}
};

public static final double[][] kPassingDistanceToAngleValues = {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/team1701/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -387,7 +387,7 @@ private void setupControllerBindings() {
mDriverController
.leftBumper()
.and(() -> mRobotState.getScoringMode().equals(ScoringMode.SPEAKER))
.whileTrue(ShootCommands.passANote(
.whileTrue(DriveCommands.passANote(
mDrive,
mShooter,
mIndexer,
Expand Down
10 changes: 6 additions & 4 deletions src/main/java/com/team1701/robot/commands/AutonomousCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ private Command driveToPoseAndPreWarm(Pose2d pose) {
return stopRoutine();
}

var setpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation()));
var setpoint = ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation()));
mPathBuilder.addPose(pose);
return DriveCommands.driveToPose(
mDrive,
Expand Down Expand Up @@ -254,7 +254,7 @@ private Command efficientlyPreWarmShootAndDrive(String pathName, String returnPa
idle(),
runOnce(() -> mRobotState.setUseAutonFallback(true)),
mRobotState::hasNote)))
.andThen(either(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback));
.andThen(either(driveToNextPiece(nextNote).withTimeout(3), none(), mRobotState::getUseAutonFallback));
}

private Command followChoreoPathAndSeekNote(String pathName) {
Expand Down Expand Up @@ -289,7 +289,8 @@ private Command followChoreoPathAndPreWarm(String pathName, boolean resetPose, b

mPathBuilder.addPath(trajectory.getPoses());

var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
var shooterSetpoint =
ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
var eventMarkers = ChoreoEventMarker.loadFromFile(pathName);
return new DriveChoreoTrajectory(
mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose)
Expand All @@ -305,7 +306,8 @@ private Command followChoreoPathAndShoot(String pathName, boolean resetPose, dou

mPathBuilder.addPath(trajectory.getPoses());

var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
var shooterSetpoint =
ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
var eventMarkers = ChoreoEventMarker.loadFromFile(pathName);
return new DriveChoreoTrajectory(
mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose)
Expand Down
22 changes: 22 additions & 0 deletions src/main/java/com/team1701/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,4 +229,26 @@ public static Command driveWithVelocity(Supplier<ChassisSpeeds> velocity, Drive
.finallyDo(() -> drive.stop())
.withName("DriveWithVelocity");
}

public static Command passANote(
Drive drive,
Shooter shooter,
Indexer indexer,
RobotState robotState,
DoubleSupplier throttleSupplier,
DoubleSupplier strafeSupplier) {
return new PassANote(
drive,
shooter,
indexer,
robotState,
() -> calculateDriveWithJoysticksVelocities(
throttleSupplier.getAsDouble(),
strafeSupplier.getAsDouble(),
drive.getFieldRelativeHeading(),
Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity())
.rotateBy(robotState.getHeading()),
() -> new Rotation2d(.03))
.withName("PassANote");
}
}
138 changes: 126 additions & 12 deletions src/main/java/com/team1701/robot/commands/PassANote.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,30 @@
package com.team1701.robot.commands;

import java.util.function.Supplier;

import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
import com.team1701.lib.util.GeometryUtil;
import com.team1701.lib.util.TimeLockedBoolean;
import com.team1701.lib.util.Util;
import com.team1701.lib.util.tuning.LoggedTunableBoolean;
import com.team1701.lib.util.tuning.LoggedTunableNumber;
import com.team1701.lib.util.tuning.LoggedTunableValue;
import com.team1701.robot.Constants;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.states.ShootingState;
import com.team1701.robot.subsystems.drive.Drive;
import com.team1701.robot.subsystems.indexer.Indexer;
import com.team1701.robot.subsystems.shooter.Shooter;
import com.team1701.robot.subsystems.shooter.Shooter.ShooterSetpoint;
import com.team1701.robot.util.FieldUtil;
import com.team1701.robot.util.ShooterUtil;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
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.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import org.littletonrobotics.junction.Logger;

Expand All @@ -21,48 +35,143 @@ public class PassANote extends Command {
new LoggedTunableBoolean(kLoggingPrefix + "TuningEnabled", false);

private static final LoggedTunableNumber kAngleToleranceRadians =
new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.01);
new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.02);

private static final TrapezoidProfile.State kZeroState = new TrapezoidProfile.State(0.0, 0.0);
private static final KinematicLimits kKinematicLimits = Constants.Drive.kFastTrapezoidalKinematicLimits;

private static final LoggedTunableNumber kMaxAngularVelocity = new LoggedTunableNumber(
kLoggingPrefix + "MaxAngularVelocity",
Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius);
private static final LoggedTunableNumber kMaxAngularAcceleration =
new LoggedTunableNumber(kLoggingPrefix + "MaxAngularAcceleration", kMaxAngularVelocity.get() / 2.0);

private Supplier<Rotation2d> mHeadingTolerance;

private final Drive mDrive;
private final Shooter mShooter;
private final Indexer mIndexer;
private final RobotState mRobotState;
private final Supplier<Translation2d> mFieldRelativeSpeeds;
private final PIDController mRotationController;

private boolean mShooting;
private TrapezoidProfile mRotationProfile;
private TrapezoidProfile.State mRotationState = kZeroState;

public PassANote(Shooter shooter, Indexer indexer, RobotState robotState) {
private boolean mShooting;
private TimeLockedBoolean mLockedReadyToShoot;

public PassANote(
Drive drive,
Shooter shooter,
Indexer indexer,
RobotState robotState,
Supplier<Translation2d> fieldRelativeSpeeds,
Supplier<Rotation2d> headingTolerance) {
mDrive = drive;
mShooter = shooter;
mIndexer = indexer;
mRobotState = robotState;
mFieldRelativeSpeeds = fieldRelativeSpeeds;
mRotationController = new PIDController(6.0, 0, 0, Constants.kLoopPeriodSeconds);
mRotationController.enableContinuousInput(-Math.PI, Math.PI);
mRotationProfile = new TrapezoidProfile(
new TrapezoidProfile.Constraints(kMaxAngularVelocity.get(), kMaxAngularAcceleration.get()));
mHeadingTolerance = headingTolerance;

mLockedReadyToShoot = new TimeLockedBoolean(.1, Timer.getFPGATimestamp());

addRequirements(shooter, indexer);
addRequirements(mDrive, shooter, indexer);
}

@Override
public void initialize() {
mShooting = false;
mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);

mRotationController.reset();
mRotationController.enableContinuousInput(-Math.PI, Math.PI);

var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity();
var headingError = mRobotState.getHeading().minus(mRobotState.getPassingHeading());
mRotationState = new TrapezoidProfile.State(
MathUtil.angleModulus(headingError.getRadians()), fieldRelativeChassisSpeeds.omegaRadiansPerSecond);

mLockedReadyToShoot.update(false, Timer.getFPGATimestamp());

LoggedTunableValue.ifChanged(
hashCode(),
() -> {
mRotationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(
Math.min(
kMaxAngularVelocity.get(),
kKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius),
Math.min(
kMaxAngularAcceleration.get(),
kKinematicLimits.maxDriveAcceleration() / Constants.Drive.kModuleRadius)));
mRotationController.setPID(6, 0, 0);
},
kMaxAngularVelocity,
kMaxAngularAcceleration);
}

@Override
public void execute() {
var setpoint = mTuningEnabled.get()
var currentPose = mRobotState.getPose2d();
var fieldRelativeSpeeds = mFieldRelativeSpeeds.get();

var droppedVelocity = ShooterUtil.calculatePassingSpeed(mRobotState.getDistanceToSpeaker())
* Constants.Shooter.kRollerSpeedToNoteSpeed;
var robotVelocityTowardsTarget = fieldRelativeSpeeds
.rotateBy(mRobotState.getPassingHeading().unaryMinus())
.getX();

var timeInAir = mRobotState.getPassingDistance() / (robotVelocityTowardsTarget + droppedVelocity);

var endTranslation = new Translation2d(
currentPose.getX() + fieldRelativeSpeeds.getX() * timeInAir,
currentPose.getY() + fieldRelativeSpeeds.getY() * timeInAir);

var shooterSetpoint = mTuningEnabled.get()
? new ShooterSetpoint(
Constants.Shooter.kTunableShooterSpeedRadiansPerSecond.get(),
Rotation2d.fromRadians(Constants.Shooter.kTunableShooterAngleRadians.get()))
: ShooterUtil.calculatePassingSetpoint(mRobotState);
: ShooterUtil.calculatePassingSetpoint(FieldUtil.getDistanceToPassTarget(endTranslation));

mShooter.setSetpoint(shooterSetpoint);

var targetHeading = mRobotState.getPassingHeading().minus(shooterSetpoint.releaseAngle());
var headingError = mRobotState.getHeading().minus(targetHeading);

Rotation2d headingSetpoint;
double rotationalVelocity;
if (GeometryUtil.isNear(GeometryUtil.kRotationIdentity, headingError, mHeadingTolerance.get())
&& Util.epsilonEquals(fieldRelativeSpeeds.getX(), 0)
&& Util.epsilonEquals(fieldRelativeSpeeds.getY(), 0)) {
rotationalVelocity = 0;
mRotationState = kZeroState;
headingSetpoint = targetHeading;
} else {
var rotationPidOutput = mRotationController.calculate(headingError.getRadians(), 0);
mRotationState = mRotationProfile.calculate(Constants.kLoopPeriodSeconds, mRotationState, kZeroState);
rotationalVelocity = mRotationState.velocity + rotationPidOutput;
headingSetpoint = Rotation2d.fromRadians(targetHeading.getRadians() + mRotationState.position);
}

mShooter.setSetpoint(setpoint);
mDrive.setVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(
fieldRelativeSpeeds.getX(), fieldRelativeSpeeds.getY(), rotationalVelocity, currentPose.getRotation()));

var atSpeed = setpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0);
var atSpeed = shooterSetpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0);

var atAngle = GeometryUtil.isNear(
mShooter.getAngle(), setpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get()));
mShooter.getAngle(), shooterSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get()));

var atHeading = GeometryUtil.isNear(
mRobotState.getPassingHeading(), mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance);
targetHeading, mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance);

var atPose = mRobotState.getPassingDistance() > 6 && !mRobotState.inOpponentWing();

if (atSpeed && atAngle && atHeading && atPose) {
if (mLockedReadyToShoot.update(atSpeed && atAngle && atHeading && atPose, Timer.getFPGATimestamp())) {
mIndexer.setForwardShoot();
mShooting = true;
}
Expand All @@ -75,13 +184,18 @@ public void execute() {
}
}

mRobotState.setShootingState(new ShootingState(setpoint, true, atAngle, atSpeed, atHeading, mShooting));
mRobotState.setShootingState(new ShootingState(shooterSetpoint, true, atAngle, atSpeed, atHeading, mShooting));
Logger.recordOutput(kLoggingPrefix + "AtPose", atPose);
Logger.recordOutput(kLoggingPrefix + "HeadingSetpoint", headingSetpoint);
}

@Override
public void end(boolean interrupted) {
mShooting = false;

mRobotState.setShootingState(ShootingState.kDefault);
mShooter.stop();
mIndexer.stop();
mDrive.stop();
}
}
2 changes: 1 addition & 1 deletion src/main/java/com/team1701/robot/commands/Shoot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ public void execute() {
currentPose.getY() + fieldRelativeSpeeds.vyMetersPerSecond * timeInAir);

var setpoint = mRobotState.isSpeakerMode()
? ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation))
? ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation))
: ShooterUtil.calculateStationarySetpoint(mRobotState);

mShooter.setSetpoint(setpoint);
Expand Down
Loading

0 comments on commit 57b88d0

Please sign in to comment.