Skip to content

Commit

Permalink
day 2 of my misery
Browse files Browse the repository at this point in the history
  • Loading branch information
@eshen7
eshen7 committed Feb 17, 2024
1 parent a112950 commit 64bf727
Show file tree
Hide file tree
Showing 19 changed files with 106 additions and 73 deletions.
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/auto/AutoCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ public Command masterSuperstructureSequence() {
return Commands.sequence(
scorePreload(),
Commands.parallel(
continuouslyIntakeForShoot(swerve::getOdoPose),
continuouslyIntakeForShoot(),
superstructure.spinUp(),
superstructure.autoFeed(
() ->
Expand Down Expand Up @@ -141,7 +141,7 @@ public Command shoot() {
return Commands.parallel(superstructure.shootStow());
}

public Command continuouslyIntakeForShoot(Supplier<Pose2d> drivePose) {
public Command continuouslyIntakeForShoot() {
return Commands.sequence(
superstructure.intake().until(() -> superstructure.getPiece()),
superstructure.passToShooterNoKicker())
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/commands/KickerCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
public class KickerCommands {

public Command kick() {
return Commands.run(() -> kicker.openLoop(0.5), kicker);
return Commands.run(() -> kicker.openLoop(0.4), kicker);
}

public Command slowFeed() {
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/commands/ShooterCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ public Command openLoop(DoubleSupplier bill) {
public Command setVelocity(DoubleSupplier bill) {
return Commands.run(
() -> {
shooterRight.setVelocity(bill.getAsDouble() * 0.8);
shooterLeft.setVelocity(bill.getAsDouble() * 1.2);
shooterRight.setVelocity(bill.getAsDouble() * 0.92);
shooterLeft.setVelocity(bill.getAsDouble() * 1.08);
},
shooterRight,
shooterLeft);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/constants/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public class FieldConstants {

public static final double kSpeakerHeight = Units.inchesToMeters(100);

public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.2, kSpeakerHeight, new Rotation3d());
public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.8, kSpeakerHeight, new Rotation3d());

public static final double kAmpHeight = Units.inchesToMeters(35);

Expand Down
14 changes: 14 additions & 0 deletions src/main/java/team3647/frc2024/constants/PivotConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.util.Units;

public class PivotConstants {
Expand Down Expand Up @@ -55,7 +56,20 @@ public class PivotConstants {
public static final TimeOfFlight tofFront =
new TimeOfFlight(GlobalConstants.SensorIds.pivotFrontId);

// distance squared vs pivot angle
public static final InterpolatingDoubleTreeMap kMasterPivotMap =
new InterpolatingDoubleTreeMap();

static {
kMasterPivotMap.put(0.0, 60.0);
kMasterPivotMap.put(1.26, 60.0);
kMasterPivotMap.put(2.0, 52.0);
kMasterPivotMap.put(2.43, 48.0);
kMasterPivotMap.put(3.0, 42.5);
kMasterPivotMap.put(3.5, 38.0);
kMasterPivotMap.put(4.0, 35.5);
kMasterPivotMap.put(4.5, 35.5);
kMasterPivotMap.put(20.0, 35.5);
Slot0Configs kMasterSlot0 = new Slot0Configs();
CurrentLimitsConfigs kMasterCurrent = new CurrentLimitsConfigs();
MotionMagicConfigs kMasterMotionMagic = new MotionMagicConfigs();
Expand Down
8 changes: 7 additions & 1 deletion src/main/java/team3647/frc2024/constants/ShooterConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public class ShooterConstants {

public static final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);

public static final double masterKP = 30;
public static final double masterKP = 1.0657;
public static final double masterKI = 0;
public static final double masterKD = 0;

Expand Down Expand Up @@ -64,10 +64,16 @@ public class ShooterConstants {
kRightSlot0.kP = masterKP;
kRightSlot0.kI = masterKI;
kRightSlot0.kD = masterKD;
kRightSlot0.kS = kS;
kRightSlot0.kV = kV;
kRightSlot0.kA = kA;

kLeftSlot0.kP = masterKP;
kLeftSlot0.kI = masterKI;
kLeftSlot0.kD = masterKD;
kLeftSlot0.kS = kS;
kLeftSlot0.kV = kV;
kLeftSlot0.kA = kA;

kRightCurrent.StatorCurrentLimitEnable = false;
kLeftCurrent.StatorCurrentLimitEnable = false;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/constants/TunerConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ public class TunerConstants {
private static final int kBackRightDriveMotorId = 7;
private static final int kBackRightSteerMotorId = 8;
private static final int kBackRightEncoderId = 12;
private static final double kBackRightEncoderOffset = 0.39892578125;
private static final double kBackRightEncoderOffset = -0.42431640625;

private static final double kBackRightXPosInches = -9.3;
private static final double kBackRightYPosInches = -9.3;
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/team3647/frc2024/constants/VisionConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,9 @@
import edu.wpi.first.math.util.Units;

public class VisionConstants {
public static final String photonIP_right = "10.36.47.14";
public static final String photonIP_left = "10.36.47.15";
public static final String photonIP_driver = "10.36.47.16";
public static final String photonIP_back = "10.36.47.14"; // has left and back left
public static final String photonIP_left = "10.36.47.15"; // has driver
public static final String photonIP_right = "10.36.47.16"; // has right and back right

public static final String backLeft = "back left";
public static final String backRight = "back right";
Expand All @@ -22,17 +22,17 @@ public class VisionConstants {
Units.inchesToMeters(-9.2193),
Units.inchesToMeters(8),
Units.inchesToMeters(8.7885)),
new Rotation3d(0, Math.PI / 180 * 28.12, 0)
.rotateBy(new Rotation3d(0, 0, Math.PI * 2 / 3)));
new Rotation3d(0, -Math.PI / 180 * 28.12, 0)
.rotateBy(new Rotation3d(0, 0, Math.PI * 5 / 6)));

public static final Transform3d robotToBackRight =
new Transform3d(
new Translation3d(
Units.inchesToMeters(-9.2193),
Units.inchesToMeters(-8),
Units.inchesToMeters(8.7885)),
new Rotation3d(0, Math.PI / 180 * 28.12, 0)
.rotateBy(new Rotation3d(0, 0, -Math.PI * 2 / 3)));
new Rotation3d(0, -Math.PI / 180 * 28.12, 0)
.rotateBy(new Rotation3d(0, 0, -Math.PI * 5 / 6)));

public static final Transform3d robotToLeft =
new Transform3d(
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public void robotInit() {
Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value

if (isReal()) {
Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
// Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
} else {
Expand Down
29 changes: 17 additions & 12 deletions src/main/java/team3647/frc2024/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import java.util.function.BooleanSupplier;
import team3647.frc2024.auto.AutoCommands;
import team3647.frc2024.auto.AutonomousMode;
Expand Down Expand Up @@ -70,15 +69,19 @@ public RobotContainer() {

private void configureButtonBindings() {

mainController.buttonX.whileTrue(superstructure.kickerCommands.unkick());
mainController.buttonX.onFalse(superstructure.kickerCommands.kill());
mainController.rightTrigger.whileTrue(autoDrive.setMode(DriveMode.SHOOT_ON_THE_MOVE));
mainController.leftTrigger.whileTrue(autoDrive.setMode(DriveMode.SHOOT_AT_AMP));
mainController
.rightTrigger
.and(() -> superstructure.getPiece())
.whileTrue(superstructure.shoot())
.onFalse(superstructure.stowFromShoot())
.onFalse(superstructure.ejectPiece());
mainController
.leftTrigger
.and(() -> superstructure.getPiece())
.whileTrue(superstructure.shoot())
.onFalse(superstructure.stowFromShoot())
.onFalse(superstructure.ejectPiece());
Expand Down Expand Up @@ -116,17 +119,17 @@ private void configureButtonBindings() {

// swerve

mainController.dPadUp.whileTrue(swerve.runDriveQuasiTest(Direction.kForward));
mainController.dPadDown.whileTrue(swerve.runDriveQuasiTest(Direction.kReverse));
// mainController.dPadUp.whileTrue(swerve.runDriveQuasiTest(Direction.kForward));
// mainController.dPadDown.whileTrue(swerve.runDriveQuasiTest(Direction.kReverse));

mainController.dPadLeft.whileTrue(swerve.runDriveDynamTest(Direction.kForward));
mainController.dPadRight.whileTrue(swerve.runDriveDynamTest(Direction.kReverse));
// mainController.dPadLeft.whileTrue(swerve.runDriveDynamTest(Direction.kForward));
// mainController.dPadRight.whileTrue(swerve.runDriveDynamTest(Direction.kReverse));

mainController.buttonY.whileTrue(swerve.runSteerQuasiTest(Direction.kForward));
mainController.buttonA.whileTrue(swerve.runSteerQuasiTest(Direction.kReverse));
// mainController.buttonY.whileTrue(swerve.runSteerQuasiTest(Direction.kForward));
// mainController.buttonA.whileTrue(swerve.runSteerQuasiTest(Direction.kReverse));

mainController.buttonX.whileTrue(swerve.runSteerDynamTest(Direction.kForward));
mainController.buttonB.whileTrue(swerve.runSteerDynamTest(Direction.kReverse));
// mainController.buttonX.whileTrue(swerve.runSteerDynamTest(Direction.kForward));
// mainController.buttonB.whileTrue(swerve.runSteerDynamTest(Direction.kReverse));

// shooter

Expand Down Expand Up @@ -178,6 +181,8 @@ public void configureSmartDashboardLogging() {
printer.addDouble("pivot", pivot::getAngle);
printer.addBoolean("under stage", swerve::underStage);
printer.addBoolean("set piece", () -> setPiece.getAsBoolean());
SmartDashboard.putNumber("pivot interp", 40);
printer.addDouble("shooter distance squared", targetingUtil::distance);
// printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta);
}

Expand Down Expand Up @@ -267,8 +272,7 @@ public Command getAutonomousCommand() {
new AprilTagPhotonVision(VisionConstants.right, VisionConstants.robotToRight);

private final VisionController visionController =
new VisionController(
swerve::addVisionData, swerve::shouldAddData, backLeft, backRight, left, right);
new VisionController(swerve::addVisionData, swerve::shouldAddData, backRight);

// private final LEDs LEDs = new LEDs(LEDConstants.m_candle);

Expand Down Expand Up @@ -309,7 +313,8 @@ public Command getAutonomousCommand() {

private final Trigger setPiece =
new Trigger(() -> intake.getMasterCurrent() > 32 && wrist.getAngle() < 5)
.debounce(0.06);
.debounce(0.06)
.or(mainController.buttonB);

private final BooleanSupplier isIntaking =
() -> mainController.leftBumper.getAsBoolean() || DriverStation.isAutonomous();
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/subsystems/ShooterLeft.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ public void setTorque(double torque) {
}

public void setVelocity(double velocity) {
super.setVelocityFOC(velocity, ff.calculate(velocity));
super.setVelocityFOC(velocity, 0);
}

public void setVoltage(double voltage) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/subsystems/ShooterRight.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ public void setTorque(double torque) {
}

public void setVelocity(double velocity) {
super.setVelocityFOC(velocity, ff.calculate(velocity));
super.setVelocityFOC(velocity, 0);
}

public void setVoltage(double voltage) {
Expand Down
23 changes: 10 additions & 13 deletions src/main/java/team3647/frc2024/subsystems/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,11 +98,10 @@ public Command shoot() {
prep(),
spinUp(),
Commands.sequence(
Commands.waitSeconds(2),
// Commands.waitUntil(() -> shooterLeft.velocityReached(shootSpeed * 1.2,
// 1))
// .withTimeout(2),
Commands.parallel(feed(), ejectPiece()).withTimeout(1)));
// Commands.waitSeconds(3),
Commands.waitUntil(() -> shooterLeft.velocityReached(shootSpeed * 1.1, 1))
.withTimeout(2),
feed()));
}

public Command shootManual() {
Expand All @@ -118,7 +117,7 @@ public Command shootStow() {
}

public Command prep() {
SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble());
// SmartDashboard.putNumber("pivot supplier", pivotAngleSupplier.getAsDouble());
return pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble());
}

Expand All @@ -142,7 +141,9 @@ public Command passToShooter() {
return Commands.parallel(
intakeCommands.kill(),
kickerCommands.kick(),
wristCommands.setAngle(() -> this.getInverseKinematics()))
wristCommands
.setAngle(() -> this.getInverseKinematics())
.until(() -> wrist.angleReached(this.getInverseKinematics(), 5)))
.withTimeout(0.5)
.andThen(shootThrough());
}
Expand Down Expand Up @@ -189,7 +190,7 @@ public Command autoFeed(BooleanSupplier goodToGo) {

public Command index() {
if (pivot.frontPiece()) {
return slightReverse().until(() -> pivot.backPiece());
return slightReverse().until(() -> !pivot.frontPiece());
} else {
return slightForwards().until(() -> pivot.frontPiece());
}
Expand All @@ -212,10 +213,6 @@ public Command slightForwards() {
}

public Command slightReverse() {
return kickerCommands
.unkick()
.until(() -> !pivot.frontPiece())
.withTimeout(0.4)
.andThen(kickerCommands.kill());
return kickerCommands.unkick();
}
}
4 changes: 2 additions & 2 deletions src/main/java/team3647/frc2024/subsystems/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -284,8 +284,8 @@ public ChassisSpeeds getFieldRelativeChassisSpeeds() {
}

public boolean shouldAddData(Pose2d visionPose) {
double distance = GeomUtil.distance(visionPose, getOdoPose());
return (distance < 1) ? true : false;
double distance = GeomUtil.distanceSquared(visionPose, getOdoPose());
return (distance < 1) ? true : true;
}

public void addVisionData(VisionMeasurement data) {
Expand Down
13 changes: 6 additions & 7 deletions src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import team3647.lib.GeomUtil;
import team3647.lib.vision.AprilTagCamera.AprilTagId;

public class AprilTagPhotonVision extends PhotonCamera implements AprilTagCamera {
Expand Down Expand Up @@ -47,12 +46,12 @@ public Optional<VisionMeasurement> QueueToInputs() {
return Optional.empty();
}
var targetDistance =
GeomUtil.distanceSquared(
result.getBestTarget()
.getBestCameraToTarget()
.getTranslation()
.toTranslation2d());
final var stdDevs = VecBuilder.fill(0.005, 0.005, 0.005).times(targetDistance);
result.getBestTarget()
.getBestCameraToTarget()
.getTranslation()
.toTranslation2d()
.getNorm();
final var stdDevs = VecBuilder.fill(0.1, 0.1, 0.1).times(targetDistance);
VisionMeasurement measurement =
VisionMeasurement.fromEstimatedRobotPose(update.get(), stdDevs);
return Optional.of(measurement);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/team3647/frc2024/util/InverseKinematics.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ public class InverseKinematics {
private final double wristLength = Units.inchesToMeters(11.2);
private final double wristRollersAngle = Math.toRadians(28);
private final Pose2d wristOriginPos = new Pose2d(wristX, wristHeight, new Rotation2d());
private final double interestingOffset = Math.toRadians(5);
private final double interestingOffset = Math.toRadians(0);

public InverseKinematics(DoubleSupplier pivotAngle) {
this.pivotAngle = pivotAngle;
Expand Down
Loading

0 comments on commit 64bf727

Please sign in to comment.