Skip to content

Commit

Permalink
Added some physical constants
Browse files Browse the repository at this point in the history
  • Loading branch information
@keckothedragon
keckothedragon committed Oct 9, 2024
1 parent 2c6194e commit 6761812
Show file tree
Hide file tree
Showing 6 changed files with 91 additions and 33 deletions.
52 changes: 42 additions & 10 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.controller.PIDController;
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;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
Expand All @@ -40,8 +40,8 @@ public final class Constants {

public static final class DriveConstants {
public static final double kMaxLinearSpeed = 6.0; // meters per second
public static final double kTrackWidthX = Units.inchesToMeters(21.5);
public static final double kTrackWidthY = Units.inchesToMeters(20.5);
public static final double kTrackWidthX = Units.inchesToMeters(15.25);
public static final double kTrackWidthY = Units.inchesToMeters(16.25);
public static final double kDriveBaseRadius =
Math.hypot(kTrackWidthX / 2.0, kTrackWidthY / 2.0);
public static final double kMaxAngularSpeed = kMaxLinearSpeed / kDriveBaseRadius;
Expand All @@ -60,7 +60,8 @@ public static final class DriveConstants {
public static final double kWheelRadius = Units.inchesToMeters(2.0);
public static final double kOdometryFrequency = 250.0;

public static final double kDriveGearRatio = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
public static final double kDriveGearRatio =
(50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0); // L3 ratio
public static final double kTurnGearRatio = 150.0 / 7.0;
}

Expand All @@ -73,13 +74,42 @@ public static final class AprilTagVisionConstants {
new LoggedTunableNumber("xyStandardDeviationCoefficient", 0.005, "Cameras");
public static final LoggedTunableNumber kThetaStandardDeviationCoefficient =
new LoggedTunableNumber("thetaStandardDeviationCoefficient", 0.01, "Cameras");
;

// TODO: Update these values
public static final Pose3d[] kCameraPoses =
new Pose3d[] {
new Pose3d(new Translation3d(), new Rotation3d()),
new Pose3d(new Translation3d(), new Rotation3d())
// transform from center of robot to camera
public static final Transform3d[] kCameraTransforms =
new Transform3d[] {
// front left (shooter)
new Transform3d(
new Translation3d(
Units.inchesToMeters(5.410),
Units.inchesToMeters(-9.454),
Units.inchesToMeters(7.766)),
new Rotation3d(
0.0, Units.degreesToRadians(55.000015), Units.degreesToRadians(-9.971306))),

// front right (shooter)
new Transform3d(
new Translation3d(
Units.inchesToMeters(5.410),
Units.inchesToMeters(9.454),
Units.inchesToMeters(7.766)),
new Rotation3d(0.0, Units.degreesToRadians(55), Units.degreesToRadians(9.971306))),

// back left (intake)
new Transform3d(
new Translation3d(
Units.inchesToMeters(-4.673),
Units.inchesToMeters(-14.620),
Units.inchesToMeters(8.585)),
new Rotation3d(0.0, Units.degreesToRadians(60), Units.degreesToRadians(180 + 10))),

// back right (intake)
new Transform3d(
new Translation3d(
Units.inchesToMeters(-4.673),
Units.inchesToMeters(14.620),
Units.inchesToMeters(8.585)),
new Rotation3d(0.0, Units.degreesToRadians(60), Units.degreesToRadians(180 - 10)))
};
}

Expand Down Expand Up @@ -111,6 +141,8 @@ public static final class KickerConstants {

public static final LoggedTunableNumber kShootingTimeout =
new LoggedTunableNumber("Kicker Shooting Timeout", 1.0);
public static final LoggedTunableNumber kIntakeTimeout =
new LoggedTunableNumber("Kicker Shooting Timeout", 1.0);

// Simulation constants
public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,9 @@ private void configureSubsystems() {
m_aprilTagVision =
new AprilTagVision(
new AprilTagVisionIONorthstar("northstar_0", ""),
new AprilTagVisionIONorthstar("northstar_1", ""));
new AprilTagVisionIONorthstar("northstar_1", ""),
new AprilTagVisionIONorthstar("northstar_2", ""),
new AprilTagVisionIONorthstar("northstar_3", ""));

if (RobotBase.isReal()) {
m_drive =
Expand Down Expand Up @@ -230,6 +232,7 @@ private void configureButtonBindings() {
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();
m_kicker.updateState(KickerState.kIdle);
}));

m_driverControls
Expand Down Expand Up @@ -283,8 +286,8 @@ private void configureButtonBindings() {

m_operatorControls
.runIntake()
.onTrue(
Commands.runOnce(
.whileTrue(
Commands.run(
() -> {
m_robotState.updateRobotAction(RobotAction.kIntake);
}))
Expand All @@ -309,6 +312,7 @@ private void configureButtonBindings() {
Commands.runOnce(
() -> {
m_robotState.setDefaultAction();
m_kicker.updateState(KickerState.kIdle);
}));

m_operatorControls
Expand Down
4 changes: 0 additions & 4 deletions src/main/java/frc/robot/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,6 @@ public void updateRobotAction(RobotAction newAction) {
case kTeleopDefault:
case kAutoDefault:
m_intake.updateState(IntakeState.kIdle);
m_kicker.updateState(KickerState.kIdle);
m_shooter.updateState(ShooterState.kIdle);
m_drive.updateProfile(DriveProfiles.kDefault);

Expand All @@ -189,15 +188,13 @@ public void updateRobotAction(RobotAction newAction) {
case kAutoShoot:
case kFeeding:
m_intake.updateState(IntakeState.kIdle);
m_kicker.updateState(KickerState.kIdle);
m_shooter.updateState(ShooterState.kRevving);
m_drive.updateProfile(DriveProfiles.kAutoAlign);

break;

case kRevNoAlign:
m_intake.updateState(IntakeState.kIdle);
m_kicker.updateState(KickerState.kIdle);
m_shooter.updateState(ShooterState.kRevving);

break;
Expand All @@ -212,7 +209,6 @@ public void updateRobotAction(RobotAction newAction) {

case kAmpLineup:
m_intake.updateState(IntakeState.kIdle);
m_kicker.updateState(KickerState.kIdle);
m_shooter.updateState(ShooterState.kAmp);
m_drive.updateProfile(DriveProfiles.kAmpLineup);
m_drive.setDesiredHeading(AllianceFlipUtil.apply(Rotation2d.fromDegrees(90)));
Expand Down
12 changes: 4 additions & 8 deletions src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
import frc.robot.Constants.FieldConstants;
import frc.robot.RobotState;
import frc.robot.subsystems.aprilTagVision.AprilTagVisionIO.AprilTagVisionInputs;
import frc.robot.util.GeomUtil;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
Expand Down Expand Up @@ -113,8 +112,7 @@ public void periodic() {

robotPose3d =
cameraPose.transformBy(
GeomUtil.toTransform3d(AprilTagVisionConstants.kCameraPoses[instanceIndex])
.inverse());
AprilTagVisionConstants.kCameraTransforms[instanceIndex].inverse());

useVisionRotation = true;
break;
Expand Down Expand Up @@ -152,12 +150,10 @@ public void periodic() {

Pose3d robotPose3d1 =
cameraPose1.transformBy(
GeomUtil.toTransform3d(AprilTagVisionConstants.kCameraPoses[instanceIndex])
.inverse());
AprilTagVisionConstants.kCameraTransforms[instanceIndex].inverse());
Pose3d robotPose3d2 =
cameraPose2.transformBy(
GeomUtil.toTransform3d(AprilTagVisionConstants.kCameraPoses[instanceIndex])
.inverse());
AprilTagVisionConstants.kCameraTransforms[instanceIndex].inverse());

double error1 = values[1];
double error2 = values[9];
Expand All @@ -182,7 +178,7 @@ public void periodic() {
}

useVisionRotation =
false; // yes i know this is technically unnecessary but i like to be explicit
false; // since there are two possible poses we shouldn't rely on the rotation

break;
}
Expand Down
20 changes: 19 additions & 1 deletion src/main/java/frc/robot/subsystems/intake/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,17 +21,35 @@ public Intake(IntakeIO io) {
m_io = io;
m_inputs = new IntakeInputsAutoLogged();

m_profiles = new SubsystemProfiles(IntakeState.class, new HashMap<>(), IntakeState.kIdle);
HashMap<Enum<?>, Runnable> periodicHash = new HashMap<>();
periodicHash.put(IntakeState.kIdle, this::idlePeriodic);
periodicHash.put(IntakeState.kIntaking, this::intakingPeriodic);
periodicHash.put(IntakeState.kVomitting, this::vomittingPeriodic);
m_profiles = new SubsystemProfiles(IntakeState.class, periodicHash, IntakeState.kIdle);
}

@Override
public void periodic() {
m_io.updateInputs(m_inputs);

m_profiles.getPeriodicFunction().run();

Logger.processInputs("Intake", m_inputs);
Logger.recordOutput("Intake/CurrentState", (IntakeState) m_profiles.getCurrentProfile());
}

private void idlePeriodic() {
m_io.setVoltage(IntakeConstants.kIdleVoltage.get());
}

private void intakingPeriodic() {
m_io.setVoltage(IntakeConstants.kIntakeVoltage.get());
}

private void vomittingPeriodic() {
m_io.setVoltage(IntakeConstants.kOuttakeVoltage.get());
}

public void updateState(IntakeState state) {
switch (state) {
case kIdle:
Expand Down
26 changes: 19 additions & 7 deletions src/main/java/frc/robot/subsystems/kicker/Kicker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ public class Kicker extends SubsystemBase {
private SubsystemProfiles m_profiles;

private Timer m_shootTimeout = new Timer();
private Timer m_intakeTimeout = new Timer();

public enum KickerState {
kIdle,
Expand All @@ -26,10 +27,10 @@ public Kicker(KickerIO io) {
m_inputs = new KickerInputsAutoLogged();

HashMap<Enum<?>, Runnable> periodicHash = new HashMap<>();
periodicHash.put(KickerState.kIdle, () -> {});
periodicHash.put(KickerState.kIdle, this::idlePeriodic);
periodicHash.put(KickerState.kIntaking, this::intakingPeriodic);
periodicHash.put(KickerState.kShooting, this::shootingPeriodic);
periodicHash.put(KickerState.kVomitting, () -> {});
periodicHash.put(KickerState.kVomitting, this::vomittingPeriodic);
m_profiles = new SubsystemProfiles(KickerState.class, periodicHash, KickerState.kIdle);
}

Expand All @@ -43,32 +44,43 @@ public void periodic() {
Logger.recordOutput("Kicker/State", (KickerState) m_profiles.getCurrentProfile());
}

private void idlePeriodic() {
m_io.setVoltage(KickerConstants.kIdleVoltage.get());
}

private void intakingPeriodic() {
if (m_io.hasGamePiece()) {
m_io.setVoltage(KickerConstants.kIntakingVoltage.get());

if (m_io.hasGamePiece() || m_intakeTimeout.hasElapsed(KickerConstants.kIntakeTimeout.get())) {
updateState(KickerState.kIdle);
m_io.setVoltage(KickerConstants.kIdleVoltage.get());
}
}

private void shootingPeriodic() {
m_io.setVoltage(KickerConstants.kShootingVoltage.get());

if (m_shootTimeout.hasElapsed(KickerConstants.kShootingTimeout.get())) {
updateState(KickerState.kIdle);
m_io.setVoltage(KickerConstants.kIdleVoltage.get());
}
}

private void vomittingPeriodic() {
m_io.setVoltage(KickerConstants.kEjectingVoltage.get());
}

public void updateState(KickerState state) {
switch (state) {
case kIdle:
m_io.setVoltage(KickerConstants.kIdleVoltage.get());
break;
case kIntaking:
m_io.setVoltage(KickerConstants.kIntakingVoltage.get());
m_intakeTimeout.restart();
break;
case kShooting:
m_io.setVoltage(KickerConstants.kShootingVoltage.get());
m_shootTimeout.restart();
break;
case kVomitting:
m_io.setVoltage(KickerConstants.kEjectingVoltage.get());
break;
}

Expand Down

0 comments on commit 6761812

Please sign in to comment.