Skip to content

Commit

Permalink
Fixed remaining errors
Browse files Browse the repository at this point in the history
  • Loading branch information
Liam Teltow committed Feb 1, 2025
1 parent b9b732b commit ccda5ba
Show file tree
Hide file tree
Showing 7 changed files with 106 additions and 453 deletions.
22 changes: 22 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,18 @@
import org.lasarobotics.utils.PIDConstants;
import org.lasarobotics.vision.AprilTagCamera.Resolution;

import com.pathplanner.lib.config.ModuleConfig;
import com.revrobotics.spark.config.SparkFlexConfig;

import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
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.Translation3d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Current;
Expand All @@ -38,6 +43,8 @@ public static class Field {
public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
public static final AprilTag BLUE_SPEAKER = getTag(7).get();
public static final AprilTag RED_SPEAKER = getTag(4).get();
public static final AprilTag BLUE_AMP = getTag(6).get();


/**
* Get AprilTag from field
Expand All @@ -55,6 +62,19 @@ public static class HID {
public static final Dimensionless CONTROLLER_DEADBAND = Units.Percent.of(10);
}


public static class WiggleStick {

public static final SparkFlexConfig WIGGLE_STICK_CONFIG = new SparkFlexConfig();

public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(10, 20);
}

public static class SmartDashboard {
public static final String SMARTDASHBOARD_DEFAULT_TAB = "SmartDashboard";
public static final String SMARTDASHBOARD_AUTO_MODE = "Auto Mode";
}

public static class Drive {
public static final DriveWheel DRIVE_WHEEL = DriveWheel.create(Units.Inches.of(4.0), Units.Value.of(1.3), Units.Value.of(1.2));
public static final PIDConstants DRIVE_PID = PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0);
Expand Down Expand Up @@ -86,6 +106,8 @@ public static class Drive {

public static final SwerveX2Module.GearRatio CTRE_GEAR_RATIO = SwerveX2Module.GearRatio.X4_3;
public static final MAXSwerveModule.GearRatio REV_GEAR_RATIO = MAXSwerveModule.GearRatio.L3;

public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(DRIVE_WHEEL.diameter.div(2), Units.MetersPerSecond.of(5.172), 1.3, DCMotor.getKrakenX60Foc(1), DRIVE_CURRENT_LIMIT, 1);
}

public static class CTREDriveHardware {
Expand Down
28 changes: 7 additions & 21 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,11 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.WaggleSubsystem;
import frc.robot.subsystems.drive.AutoTrajectory;
import frc.robot.subsystems.drive.CTREDriveSubsystem;
import frc.robot.subsystems.drive.REVDriveSubsystem;

public class RobotContainer {
private static final CTREDriveSubsystem CTREDRIVE_SUBSYSTEM = new CTREDriveSubsystem(
private static final CTREDriveSubsystem CTRE_DRIVE_SUBSYSTEM = new CTREDriveSubsystem(
CTREDriveSubsystem.initializeHardware(),
Constants.Drive.DRIVE_ROTATE_PID,
Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
Expand All @@ -27,7 +26,7 @@ public class RobotContainer {
Constants.Drive.DRIVE_LOOKAHEAD
);

private static final REVDriveSubsystem REVDRIVE_SUBSYSTEM = new REVDriveSubsystem(
private static final REVDriveSubsystem REV_DRIVE_SUBSYSTEM = new REVDriveSubsystem(
REVDriveSubsystem.initializeHardware(),
Constants.Drive.DRIVE_ROTATE_PID,
Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
Expand All @@ -46,16 +45,16 @@ public class RobotContainer {

public RobotContainer() {
// Set drive command
CTREDRIVE_SUBSYSTEM.setDefaultCommand(
CTREDRIVE_SUBSYSTEM.driveCommand(
CTRE_DRIVE_SUBSYSTEM.setDefaultCommand(
CTRE_DRIVE_SUBSYSTEM.driveCommand(
() -> PRIMARY_CONTROLLER.getLeftY(),
() -> PRIMARY_CONTROLLER.getLeftX(),
() -> PRIMARY_CONTROLLER.getRightX()
)
);

// Setup AutoBuilder
CTREDRIVE_SUBSYSTEM.configureAutoBuilder();
CTRE_DRIVE_SUBSYSTEM.configureAutoBuilder();

autoModeChooser();
SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser);
Expand All @@ -66,19 +65,9 @@ public RobotContainer() {

private void configureBindings() {
// Start button - toggle traction control
PRIMARY_CONTROLLER.start().onTrue(CTREDRIVE_SUBSYSTEM.toggleTractionControlCommand());
PRIMARY_CONTROLLER.start().onTrue(CTRE_DRIVE_SUBSYSTEM.toggleTractionControlCommand());

// A button - go to amp
PRIMARY_CONTROLLER.a().whileTrue(
CTREDRIVE_SUBSYSTEM.goToPoseCommand(
Constants.Field.AMP
)
);

// B button - go to source
PRIMARY_CONTROLLER.b().whileTrue(CTREDRIVE_SUBSYSTEM.goToPoseCommand(Constants.Field.SOURCE));

PRIMARY_CONTROLLER.povLeft().onTrue(CTREDRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
PRIMARY_CONTROLLER.povLeft().onTrue(CTRE_DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));

// Left/right bumper - wiggle stick
PRIMARY_CONTROLLER.leftBumper().onTrue(WIGGLE_STICK.setPositionCommand(0.0));
Expand All @@ -90,9 +79,6 @@ private void configureBindings() {
*/
private void autoModeChooser() {
m_automodeChooser.setDefaultOption("Do nothing", Commands.none());
m_automodeChooser.addOption("Leave", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Leave").getCommand());
m_automodeChooser.addOption("Preload + 3 Ring", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Preload + 3 Ring").getCommand());
m_automodeChooser.addOption("Preload + 1", new AutoTrajectory(CTREDRIVE_SUBSYSTEM, "Preload + 1").getCommand());
}

/**
Expand Down
31 changes: 19 additions & 12 deletions src/main/java/frc/robot/subsystems/WaggleSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,13 @@
package frc.robot.subsystems;

import org.lasarobotics.hardware.revrobotics.Spark;
import org.lasarobotics.hardware.revrobotics.Spark.FeedbackSensor;
import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;

import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.config.SparkBaseConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;

import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -18,33 +20,38 @@

public class WaggleSubsystem extends SubsystemBase {
Spark m_motor;
SparkPIDConfig m_config;
SparkBaseConfig m_config;
Constraints m_constraint;

/** Creates a new wiggleStick. */
public WaggleSubsystem(SparkPIDConfig config, Constraints constraint) {
public WaggleSubsystem(SparkBaseConfig config, Constraints constraint) {
m_motor = new Spark(new Spark.ID("wiggleStick", 20), MotorKind.NEO);
m_config = config;
m_constraint = constraint;

m_config = (m_motor.getKind().equals(MotorKind.NEO_VORTEX)) ? new SparkFlexConfig() : new SparkMaxConfig();
double conversionFactor = 1.0;
m_motor.initializeSparkPID(config, FeedbackSensor.NEO_ENCODER);
m_motor.setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
m_motor.setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, conversionFactor);
m_config.encoder.positionConversionFactor(conversionFactor);
m_config.encoder.velocityConversionFactor(conversionFactor / 60);
m_config.closedLoop.pidf(0.2, 0.0, 0.0, 0);
m_config.closedLoop.maxMotion.maxAcceleration(20);
m_config.closedLoop.maxMotion.maxVelocity(10);

m_motor.enablePIDWrapping(0.0, 15.0);
m_config.closedLoop.positionWrappingEnabled(true);
m_motor.setIdleMode(IdleMode.kCoast);
m_motor.resetEncoder();
}

public void setPosition(double position) {
m_motor.smoothMotion(position, m_constraint);
/** Moves to the given position */
private void setPosition(double position) {
m_motor.set(position, ControlType.kMAXMotionPositionControl);
}

/** Gets the position using the encoders */
public double getPosition() {
return m_motor.getInputs().encoderPosition;
}

/** Command to set the position of the robot */
public Command setPositionCommand(double position) {
return runOnce(() -> setPosition(position));

Expand Down
19 changes: 13 additions & 6 deletions src/main/java/frc/robot/subsystems/drive/AutoTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,17 @@

package frc.robot.subsystems.drive;

import java.io.IOException;
import java.util.List;
import java.util.Optional;

import org.json.simple.parser.ParseException;
import org.lasarobotics.drive.swerve.SwerveDrive;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.IdealStartingState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;

Expand All @@ -28,8 +32,10 @@ public class AutoTrajectory {
* Create new path trajectory using PathPlanner path
* @param driveSubsystem DriveSubsystem to drive the robot
* @param autoName PathPlanner auto name
* @throws IOException if attempting to load a file that does not exist or cannot be read
* @throws ParseException If JSON within file cannot be parsed
*/
public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) {
public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) throws IOException, ParseException {
this.m_driveSubsystem = driveSubsystem;

// Get path
Expand All @@ -44,18 +50,19 @@ public AutoTrajectory(SwerveDrive driveSubsystem, String autoName) {
*/
public AutoTrajectory(SwerveDrive driveSubsystem, List<Pose2d> waypoints, PathConstraints pathConstraints) {
this.m_driveSubsystem = driveSubsystem;
IdealStartingState startingState = new IdealStartingState(0, null);

// Generate path from waypoints
m_auto = new Pair<String, List<PathPlannerPath>>("", List.of(new PathPlannerPath(
PathPlannerPath.bezierFromPoses(waypoints),
PathPlannerPath.waypointsFromPoses(waypoints),
pathConstraints,
new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).getRotation())
startingState, new GoalEndState(0.0, waypoints.get(waypoints.size() - 1).getRotation())
)));
}

/** Return initial pose */
public Pose2d getInitialPose() {
return m_auto.getSecond().get(0).getPreviewStartingHolonomicPose();
public Optional<Pose2d> getInitialPose() {
return m_auto.getSecond().get(0).getStartingHolonomicPose();
}

/**
Expand All @@ -68,7 +75,7 @@ public Command getCommand() {
: new PathPlannerAuto(m_auto.getFirst());

return Commands.sequence(
m_driveSubsystem.resetPoseCommand(() -> getInitialPose()),
m_driveSubsystem.resetPoseCommand(() -> getInitialPose().orElseThrow(null)),
autoCommand,
m_driveSubsystem.stopCommand(),
m_driveSubsystem.lockCommand()
Expand Down
45 changes: 45 additions & 0 deletions src/main/java/frc/robot/subsystems/drive/CTREDriveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,23 @@
import org.lasarobotics.hardware.kauailabs.NavX2;
import org.lasarobotics.utils.PIDConstants;
import org.lasarobotics.vision.AprilTagCamera;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.Dimensionless;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.Constants;

public class CTREDriveSubsystem extends SwerveDrive {

Hardware m_drivetrainHardware;
/**
* Create an instance of DriveSubsystem
* <p>
Expand All @@ -40,6 +48,7 @@ public CTREDriveSubsystem(Hardware drivetrainHardware, PIDConstants rotatePIDF,
PolynomialSplineFunction throttleInputCurve, PolynomialSplineFunction rotateInputCurve,
Angle rotateScalar, Dimensionless deadband, Time lookAhead) {
super(drivetrainHardware, rotatePIDF, aimPIDF, controlCentricity, throttleInputCurve, rotateInputCurve, rotateScalar, deadband, lookAhead);
m_drivetrainHardware = drivetrainHardware;
}

/**
Expand Down Expand Up @@ -157,4 +166,40 @@ public static Hardware initializeHardware() {

return drivetrainHardware;
}

/**Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos.
* Uses the PathPlanner AutoBuilder method.
*/

public void configureAutoBuilder() {
AutoBuilder.configure(
() -> getPose(), // Robot pose supplier
(pose) -> resetPose(pose), // Method to reset odometry (will be called if your auto has a starting pose)
() -> getChassisSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> autoDrive(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
),
new RobotConfig(Constants.Drive.MASS, // The robot configuration
Units.KilogramSquareMeters.of(1),
Constants.Drive.MODULE_CONFIG,
m_drivetrainHardware.lFrontModule().getModuleCoordinate(),
m_drivetrainHardware.rFrontModule().getModuleCoordinate(),
m_drivetrainHardware.lRearModule().getModuleCoordinate(),
m_drivetrainHardware.rRearModule().getModuleCoordinate()),
() -> {
// Boolean supplier that controls when the path will be mirrored for the red alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE

var alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this // Reference to this subsystem to set requirements
);
}
}
Loading

0 comments on commit ccda5ba

Please sign in to comment.