Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/main' into fallback-trajectories
Browse files Browse the repository at this point in the history
  • Loading branch information
@danjburke12
danjburke12 committed Apr 4, 2024
2 parents d4f4ddb + d8e6b83 commit e27479d
Show file tree
Hide file tree
Showing 8 changed files with 144 additions and 94 deletions.
2 changes: 1 addition & 1 deletion src/main/java/com/team1701/lib/drivers/cameras/apriltag/AprilTagCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ private Pose3d[] getRealTargetPoses(AprilTagPipelineResult pipelineResult) {

private Optional<EstimatedRobotPose> getEstimatedPose(AprilTagPipelineResult pipelineResult) {
if (pipelineResult.multiTargetResult.isPresent()
&& pipelineResult.multiTargetResult.get().reprojectionError < 0.6) {
&& pipelineResult.multiTargetResult.get().cameraPose.getZ() < 1.0) {
var multiTargetResult = pipelineResult.multiTargetResult.get();
var fieldLayout = mFieldLayoutSupplier.get();
var stdDevs = getStdDevs(multiTargetResult.targetIds, multiTargetResult.cameraPose);
Expand Down
58 changes: 29 additions & 29 deletions src/main/java/com/team1701/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,7 +259,7 @@ public static final class Drive {

switch (Configuration.getRobot()) {
case COMPETITION_BOT:
kWheelRadiusMeters = Units.inchesToMeters(1.95379394);
kWheelRadiusMeters = Units.inchesToMeters(1.9575);
driveMotorMaxRPM = 5500; // Measured + 10%
turnMotorMaxRPM = Constants.Motors.kMaxKrakenRPM;
kDriveReduction = k16ToothKitReduction * kL3DriveReduction;
Expand Down Expand Up @@ -461,37 +461,37 @@ public static final class Shooter {
public static final boolean kUseNewCurves = true;

public static final double[][] kShooterDistanceToAngleValues = {
{2.32, 1}, // 40 in
{2.69, .83}, // 67 in
{3.01, .77}, // 85.75 in
{3.48, .64}, // 110.75 in
{3.85, .62}, // 123.5 in
{3.95, .58}, // 136.5 in
{4.4, .52}, // 158 in
{4.77, .48}, // 173.5 in
{5.4, .455}, // 203 in
{5.7, .4325}, // 218 in
{6.17, .41}, // 240 in
{6.88, .382}, // 270 in
{7.24, .37}, // 285 in
{7.7, .357} // 300 in
{2.34, 1}, // 3.875 in
{2.716, .83}, // 30.875 in
{3.034, .77}, // 49.625 in
{3.501, .64}, // 74.625 in
{3.766, .62}, // 87.375 in
{4.085, .58}, // 100.375 in
{4.4, .52}, // 121.875 in
{4.805, .48}, // 137.375 in
{5.441, .455}, // 166.875 in
{5.984, .4325}, // 181.875 in
{6.27, .41}, // 203.875 in
{7.172, .382}, // 233.875 in
{7.753, .37}, // 248.875 in
{8.017, .357} // 263.875 in
};

public static final double[][] kShooterDistanceToSpeedValues = {
{2.32, 300}, // 300
{2.69, 330}, // 330
{3.01, 375}, // 375
{3.48, 390}, // 390
{3.85, 410}, // 410
{3.95, 430}, // 430
{4.4, 450}, // 450
{4.77, 480}, // 480
{5.4, 490},
{5.7, 510},
{6.17, 530},
{6.88, 550},
{7.24, 560},
{7.7, 570}
{2.34, 300},
{2.716, 330},
{3.034, 375},
{3.501, 390},
{3.766, 410},
{4.085, 430},
{4.4, 450},
{4.805, 480},
{5.441, 490},
{5.984, 510},
{6.27, 530},
{7.172, 550},
{7.753, 560},
{8.017, 570}
};

public static final double[][] kShooterForwardVelocityToHeadingOffset = {
Expand Down
52 changes: 23 additions & 29 deletions src/main/java/com/team1701/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@
import com.team1701.robot.subsystems.intake.Intake;
import com.team1701.robot.subsystems.leds.LED;
import com.team1701.robot.subsystems.shooter.Shooter;
import com.team1701.robot.subsystems.shooter.Shooter.ShooterOffset;
import com.team1701.robot.subsystems.vision.Vision;
import com.team1701.robot.util.SparkMotorFactory;
import com.team1701.robot.util.SparkMotorFactory.MotorUsage;
Expand Down Expand Up @@ -140,7 +141,8 @@ public RobotContainer() {
Constants.Shooter.kShooterRightLowerRollerMotorId, MotorUsage.SHOOTER_ROLLER, true),
SparkMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterRotationMotorId, MotorUsage.ROTATION, false),
new EncoderIORevThroughBore(Constants.Shooter.kShooterThroughBoreEncoderId, true)));
new EncoderIORevThroughBore(Constants.Shooter.kShooterThroughBoreEncoderId, true),
mRobotState));

indexer = Optional.of(new Indexer(
SparkMotorFactory.createIndexerMotorIOSparkFlex(Constants.Indexer.kIndexerMotorId),
Expand Down Expand Up @@ -197,7 +199,8 @@ public RobotContainer() {
Shooter.createRollerMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createRollerMotorSim(DCMotor.getNeoVortex(1)),
rotationMotor,
Shooter.createEncoderSim(rotationMotor)));
Shooter.createEncoderSim(rotationMotor),
mRobotState));

indexer = Optional.of(new Indexer(
new MotorIOSim(
Expand Down Expand Up @@ -280,8 +283,8 @@ public RobotContainer() {
},
new DetectorCameraIO[] {() -> Constants.Vision.kLimelightConfig}));

mShooter = shooter.orElseGet(
() -> new Shooter(new MotorIO() {}, new MotorIO() {}, new MotorIO() {}, new EncoderIO() {}));
mShooter = shooter.orElseGet(() ->
new Shooter(new MotorIO() {}, new MotorIO() {}, new MotorIO() {}, new EncoderIO() {}, mRobotState));

mIndexer = indexer.orElseGet(() -> new Indexer(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {}));

Expand Down Expand Up @@ -468,21 +471,18 @@ private void setupControllerBindings() {
var setClimbModeCommand = runOnce(() -> mRobotState.setScoringMode(ScoringMode.CLIMB))
.ignoringDisable(true)
.withName("SetClimbScoringMode");
var shooterUpCommand = run(
() -> {
mShooter.setShooterUp();
},
mShooter)
.withName("StreamDeckShooterUpCommand");
var shooterDownCommand = startEnd(
() -> {
mShooter.setShooterDown();
},
() -> {
mShooter.stopRotation();
},
mShooter)
.withName("StreamDeckShooterDownCommand");
// if shots are low/normal, use high
var useHighCommand = startEnd(
() -> Shooter.mShooterOffset = ShooterOffset.kUseHigh,
() -> Shooter.mShooterOffset = ShooterOffset.kUseDefault)
.ignoringDisable(true)
.withName("StreamDeckUseHighCommand");
// but if shots are high, use low
var useLowCommand = startEnd(
() -> Shooter.mShooterOffset = ShooterOffset.kUseLow,
() -> Shooter.mShooterOffset = ShooterOffset.kUseDefault)
.ignoringDisable(true)
.withName("StreamDeckUseLowCommand");
var manualShootCommand =
ShootCommands.manualShoot(mShooter, mIndexer, mRobotState).withName("StreamDeckShootCommand");
var extendWinchCommand = run(
Expand All @@ -509,8 +509,8 @@ private void setupControllerBindings() {
.add(StreamDeckButton.kLeftClimbButton, leftClimbCommand::isScheduled)
.add(StreamDeckButton.kRightClimbButton, rightClimbCommand::isScheduled)
.add(StreamDeckButton.kCenterClimbButton, centerClimbCommand::isScheduled)
.add(StreamDeckButton.kShooterUpButton, shooterUpCommand::isScheduled)
.add(StreamDeckButton.kShooterDownButton, shooterDownCommand::isScheduled)
.add(StreamDeckButton.kUseHighButton, () -> Shooter.mShooterOffset == ShooterOffset.kUseHigh)
.add(StreamDeckButton.kUseLowButton, () -> Shooter.mShooterOffset == ShooterOffset.kUseLow)
.add(StreamDeckButton.kShootButton, manualShootCommand::isScheduled)
.add(StreamDeckButton.kExtendWinchButton, extendWinchCommand::isScheduled)
.add(StreamDeckButton.kRetractWinchButton, retractWinchCommand::isScheduled)
Expand All @@ -525,14 +525,8 @@ private void setupControllerBindings() {
mStreamDeck.button(StreamDeckButton.kRightClimbButton).onTrue(rightClimbCommand);
mStreamDeck.button(StreamDeckButton.kCenterClimbButton).onTrue(centerClimbCommand);

mStreamDeck
.button(StreamDeckButton.kShooterUpButton)
.whileTrue(shooterUpCommand)
.onFalse(stopShooterCommand);
mStreamDeck
.button(StreamDeckButton.kShooterDownButton)
.whileTrue(shooterDownCommand)
.onFalse(stopShooterCommand);
mStreamDeck.button(StreamDeckButton.kUseHighButton).toggleOnTrue(useHighCommand);
mStreamDeck.button(StreamDeckButton.kUseLowButton).toggleOnTrue(useLowCommand);
mStreamDeck
.button(StreamDeckButton.kShootButton)
.whileTrue(manualShootCommand)
Expand Down
24 changes: 21 additions & 3 deletions src/main/java/com/team1701/robot/commands/AutonomousCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,11 @@ public AutonomousCommands(RobotState robotState, Drive drive, Shooter shooter, I
}

private Pose2d autoFlipPose(Pose2d pose) {
var flippedPose = GeometryUtil.flipX(pose, FieldConstants.kFieldLongLengthMeters);
return Configuration.isRedAlliance() ? flippedPose : pose;
return Configuration.isRedAlliance() ? GeometryUtil.flipX(pose, FieldConstants.kFieldLongLengthMeters) : pose;
}

private Rotation2d autoFlipRotation(Rotation2d rotation) {
return Configuration.isRedAlliance() ? GeometryUtil.flipX(rotation) : rotation;
}

private Command idleShooter() {
Expand Down Expand Up @@ -227,6 +230,17 @@ private Pose2d getFirstPose(String pathName) {
return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getInitialPose();
}

private Rotation2d getInitialVelocityHeading(String pathName) {
var trajectory = Choreo.getTrajectory(pathName);
if (trajectory == null) {
return GeometryUtil.kRotationIdentity;
}

// Velocity of initial state is 0
var firstState = trajectory.sample(Constants.kLoopPeriodSeconds);
return new Rotation2d(firstState.velocityX, firstState.velocityY);
}

private Pose2d getFinalPose(String pathName) {
var trajectory = Choreo.getTrajectory(pathName);
return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getFinalPose();
Expand Down Expand Up @@ -310,7 +324,11 @@ private Command aimAndShoot() {
}

private Command pauseDrive(String pathName) {
return new PauseDrive(mDrive, mRobotState, () -> getFirstPose(pathName));
var moduleHeading = getInitialVelocityHeading(pathName);
var robotPose = getFirstPose(pathName).getRotation();
return new PauseDrive(
mDrive, mRobotState, () -> autoFlipRotation(moduleHeading), () -> autoFlipRotation(robotPose))
.withName("PauseDrive");
}

public AutonomousCommand demo() {
Expand Down
49 changes: 26 additions & 23 deletions src/main/java/com/team1701/robot/commands/PauseDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,29 +2,37 @@

import java.util.function.Supplier;

import com.team1701.lib.util.GeometryUtil;
import com.team1701.robot.Constants;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Subsystem;
import org.littletonrobotics.junction.Logger;

public class PauseDrive extends Command {
private static final String kLoggingPrefix = "Command/PauseDrive/";
private final Drive mDrive;
private final RobotState mRobotState;
private Supplier<Pose2d> mTargetPoseSupplier;
private Command mRotationCommand;
private final Supplier<Rotation2d> mTargetModuleHeadingSupplier;
private final Command mRotationCommand;

public PauseDrive(Drive drive, RobotState robotState, Supplier<Pose2d> targetPoseSupplier) {
private Rotation2d mTargetModuleHeading = GeometryUtil.kRotationIdentity;

public PauseDrive(
Drive drive,
RobotState robotState,
Supplier<Rotation2d> targetModuleHeadingSupplier,
Supplier<Rotation2d> targetRobotHeadingSupplier) {
mDrive = drive;
mRobotState = robotState;
mTargetPoseSupplier = targetPoseSupplier;
mTargetModuleHeadingSupplier = targetModuleHeadingSupplier;
mRotationCommand = new RotateToFieldHeading(
drive,
mTargetPoseSupplier.get()::getRotation,
targetRobotHeadingSupplier::get,
mRobotState::getHeading,
mRobotState::getToleranceSpeakerHeading,
Constants.Drive.kFastTrapezoidalKinematicLimits,
Expand All @@ -37,33 +45,28 @@ public PauseDrive(Drive drive, RobotState robotState, Supplier<Pose2d> targetPos
@Override
public void initialize() {
mRotationCommand.initialize();
mTargetModuleHeading = mTargetModuleHeadingSupplier.get();
Logger.recordOutput(kLoggingPrefix + "TargetModuleHeading", mTargetModuleHeading);
}

@Override
public void execute() {
if (Math.hypot(
mDrive.getFieldRelativeVelocity().vxMetersPerSecond,
mDrive.getFieldRelativeVelocity().vyMetersPerSecond)
< 0.5) {

mDrive.orientModules(mTargetPoseSupplier
.get()
.getTranslation()
.minus(mRobotState.getPose2d().getTranslation())
.getAngle()
.rotateBy(mRobotState.getHeading()));
var velocity = mDrive.getVelocity();
if (Math.hypot(velocity.vxMetersPerSecond, velocity.vyMetersPerSecond) < 0.5) {
mDrive.orientModules(mTargetModuleHeading.minus(mRobotState.getHeading()));
} else {
mRotationCommand.execute();
}
}

@Override
public void end(boolean interrupted) {
Logger.recordOutput(kLoggingPrefix + "TargetModuleHeading", GeometryUtil.kRotationIdentity);
}

@Override
public boolean isFinished() {
return MathUtil.isNear(
0,
Math.hypot(
mDrive.getFieldRelativeVelocity().vxMetersPerSecond,
mDrive.getFieldRelativeVelocity().vyMetersPerSecond),
0.1);
var velocity = mDrive.getVelocity();
return MathUtil.isNear(0, Math.hypot(velocity.vxMetersPerSecond, velocity.vyMetersPerSecond), 0.1);
}
}
4 changes: 2 additions & 2 deletions src/main/java/com/team1701/robot/controls/StreamDeck.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,8 @@ public static enum StreamDeckButton {
kLeftClimbButton(0, 1, "Controls/LeftClimbButton", "LeftClimbIcon", "Left"),
kRightClimbButton(2, 1, "Controls/RightClimbButton", "RightClimbIcon", "Right"),
kCenterClimbButton(1, 1, "Controls/CenterClimbButton", "CenterClimbIcon", "Center"),
kShooterUpButton(0, 3, "Controls/ShooterUpButton", "RaiseShooterIcon", "Raise"),
kShooterDownButton(2, 3, "Controls/ShooterDownButton", "LowerShooterIcon", "Lower"),
kUseHighButton(0, 3, "Controls/ShooterUpButton", "RaiseShooterIcon", "Use High"),
kUseLowButton(2, 3, "Controls/ShooterDownButton", "LowerShooterIcon", "Use Low"),
kShootButton(1, 2, "Controls/ShootButton", "ShootIcon", "Shoot"),
kStopShootButton(1, 3, "Controls/StopShootButton", "StopShooterIcon", "Stop"),
kSpeakerModeButton(0, 4, "Controls/SpeakerModeButton", "SpeakerModeIcon", "Speaker"),
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/com/team1701/robot/simulation/NoteSimulator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ public class NoteSimulator extends SubsystemBase {
private final VisionConfig mDetectorVisionConfig;
private Intake mIntake = new Intake(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {});
private Indexer mIndexer = new Indexer(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {});
private Shooter mShooter = new Shooter(new MotorIO() {}, new MotorIO() {}, new MotorIO() {}, new EncoderIO() {});
private Shooter mShooter;

private final List<NoteOnField> mNotesOnField = new ArrayList<>();
private final List<NoteInRobot> mNotesInRobot = new ArrayList<>();
Expand All @@ -82,6 +82,7 @@ public record NoteSimulatorSensors(

public NoteSimulator(RobotState robotState, VisionConfig detectorVisionConfig) {
mRobotState = robotState;
mShooter = new Shooter(new MotorIO() {}, new MotorIO() {}, new MotorIO() {}, new EncoderIO() {}, mRobotState);
mDetectorVisionConfig = detectorVisionConfig;

mNotesInRobot.add(new NoteInRobot(kIndexerPathLength / 2, NoteLocation.INTAKE));
Expand Down
Loading

0 comments on commit e27479d

Please sign in to comment.