Skip to content

Commit

Permalink
converted things to constants and checked some fb gains
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Feb 12, 2024
1 parent 8f7cc10 commit 25b9d55
Show file tree
Hide file tree
Showing 5 changed files with 36 additions and 30 deletions.
28 changes: 22 additions & 6 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@

package frc.robot;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
Expand Down Expand Up @@ -52,15 +55,23 @@ private DriveConstants() {
throw new IllegalStateException("Constants class should not be constructed");
}


// module constants
public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);

public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18.0);
public static final double TRACK_WIDTH_X = Units.inchesToMeters(20.733);
public static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.733);
public static final double DRIVE_BASE_RADIUS =
Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;

// kV, kS, kA in order
protected static final double[] DRIVE_FF_GAINS = new double[]{0.13, 0.1, 0.0};
// kP, kI, kD in order
protected static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0};
protected static final double[] DRIVE_FB_GAINS = new double[]{0.314, 0.0, 0.0};
// kP, kI, kD in order
protected static final double[] TURN_FB_GAINS = new double[]{0.1, 0.0, 0.0};
protected static final double[] TURN_FB_GAINS = new double[]{43.982, 0.0, 0.0};

// public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d(
// new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(8.5), Units.inchesToMeters(6)),
Expand All @@ -75,18 +86,23 @@ private DriveConstants() {
new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(9.0), Units.inchesToMeters(6.0)),
new Rotation3d(Units.degreesToRadians(5.0), Units.degreesToRadians(-28.125), Units.degreesToRadians(35.0 + 180))
);

public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d(
new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-9.0), Units.inchesToMeters(6.0)),
new Rotation3d(Units.degreesToRadians(2.0), Units.degreesToRadians(-26.0), Units.degreesToRadians(-35.0 - 180))
);

public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints(
Units.feetToMeters(19.5),
Units.feetToMeters(19.5),
360,
360
Units.radiansToDegrees(MAX_LINEAR_SPEED),
Units.radiansToDegrees(MAX_LINEAR_SPEED),
MAX_ANGULAR_SPEED,
MAX_ANGULAR_SPEED
);

public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig(
new PIDConstants(5.0),new PIDConstants(5.0),
DriveConstants.MAX_LINEAR_SPEED, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig());

public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants(
0,
new int[]{0, 1, 2}, // drive, turn, encoder
Expand Down
5 changes: 2 additions & 3 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,10 +62,9 @@ public void robotInit() {

// Set up data receivers & replay source
switch (Constants.currentMode) {
case PROTO_ARM:
case REAL, PROTO_SHOOTER:
case REAL, PROTO_SHOOTER, PROTO_ARM:
// Running on a real robot, log to a USB stick ("/U/logs")
// Logger.addDataReceiver(new WPILOGWriter("/media/sda1/aodea"));
Logger.addDataReceiver(new WPILOGWriter("/media/sda1/aoede"));
Logger.addDataReceiver(new NT4Publisher());
break;

Expand Down
29 changes: 10 additions & 19 deletions src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,30 +33,22 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.Constants.DriveConstants;
import frc.robot.subsystems.drive.module.Module;
import frc.robot.subsystems.drive.module.ModuleIO;
import frc.robot.subsystems.vision.VisionSubsystem;
import frc.robot.util.LocalADStarAK;

import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.stream.Stream;

import frc.robot.util.PoseEstimator;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

public class DriveSubsystem extends SubsystemBase {
private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
private static final double DRIVE_BASE_RADIUS =
Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;

public static final Lock odometryLock = new ReentrantLock();
private final GyroIO gyroIO;
Expand Down Expand Up @@ -112,8 +104,7 @@ public DriveSubsystem(
this::setPose,
() -> kinematics.toChassisSpeeds(getModuleStates()),
this::runVelocity,
new HolonomicPathFollowerConfig(
MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
DriveConstants.HOLONOMIC_CONFIG,
() ->
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red,
Expand Down Expand Up @@ -201,7 +192,7 @@ public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED);

// Send setpoints to modules
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
Expand Down Expand Up @@ -283,25 +274,25 @@ public void setPose(Pose2d pose) {

/** Returns the maximum linear speed in meters per sec. */
public double getMaxLinearSpeedMetersPerSec() {
return MAX_LINEAR_SPEED;
return DriveConstants.MAX_LINEAR_SPEED;
}

/** Returns the maximum angular speed in radians per sec. */
public double getMaxAngularSpeedRadPerSec() {
return MAX_ANGULAR_SPEED;
return DriveConstants.MAX_ANGULAR_SPEED;
}

public Command pathfollowFactory(Pose2d pose) {
return AutoBuilder.pathfindToPose(pose, Constants.DriveConstants.DEFAULT_CONSTRAINTS);
return AutoBuilder.pathfindToPose(pose, DriveConstants.DEFAULT_CONSTRAINTS);
}

/** Returns an array of module translations. */
public static Translation2d[] getModuleTranslations() {
return new Translation2d[] {
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
new Translation2d(DriveConstants.TRACK_WIDTH_X / 2.0, DriveConstants.TRACK_WIDTH_Y / 2.0),
new Translation2d(DriveConstants.TRACK_WIDTH_X / 2.0, -DriveConstants.TRACK_WIDTH_Y / 2.0),
new Translation2d(-DriveConstants.TRACK_WIDTH_X / 2.0, DriveConstants.TRACK_WIDTH_Y / 2.0),
new Translation2d(-DriveConstants.TRACK_WIDTH_X / 2.0, -DriveConstants.TRACK_WIDTH_Y / 2.0)
};
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ public void updateInputs(ModuleIOInputs inputs) {

@Override
public void setDriveVelocityMPS(double mps) {
double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS());
double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * m_moduleConstants.DRIVE_GEAR_RATIO();
VelocityVoltage velRequest = new VelocityVoltage(rps).withSlot(0);
m_driveTalon.setControl(velRequest.withVelocity(rps));
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public VisionSubsystem(String camName, Transform3d camPose) {
m_aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
m_photonPoseEstimator = new PhotonPoseEstimator(
m_aprilTagFieldLayout,
PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY,
m_camera,
robotToCam);
} catch (IOException e){
Expand Down

0 comments on commit 25b9d55

Please sign in to comment.