Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into 2024x2025

Browse Source
This commit is contained in:
C4llSiqn
2025-01-17 20:39:20 -07:00
parent 9db9d25f5b e8fe7a1d52
commit a8c91b3ce1
7 changed files with 394 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/New Auto.auto
+19
View File
@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Example Path"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/navgrid.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/pathplanner/paths/Example Path.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.415049342105263,
"y": 4.785115131578947
},
"prevControl": null,
"nextControl": {
"x": 2.905756578947368,
"y": 4.794736842105262
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.8294407894736837,
"y": 5.862746710526316
},
"prevControl": {
"x": 3.0693256578947365,
"y": 5.872368421052631
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -2.4366482468102095
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/settings.json
+32
View File
@@ -0,0 +1,32 @@
{
"robotWidth": 0.9,
"robotLength": 0.9,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 22.6796,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.048,
"driveGearing": 5.143,
"maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0,
"wheelCOF": 1.2,
"flModuleX": 0.273,
"flModuleY": 0.273,
"frModuleX": 0.273,
"frModuleY": -0.273,
"blModuleX": -0.273,
"blModuleY": 0.273,
"brModuleX": -0.273,
"brModuleY": -0.273,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
}
src/main/java/frc4388/robot/Constants.java
+36 -11
View File
@@ -102,51 +102,76 @@ public final class Constants {
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
// public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
// public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
private static final class ModuleSpecificConstants {
private static final class ModuleSpecificConstants { //2025
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
}
/* private static final class ModuleSpecificConstants { // 2024
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
// private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
// private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
// private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
// private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
// private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
// private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
// private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
// private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
}
} */
public static final class IDs {
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
src/main/java/frc4388/robot/RobotContainer.java
+15 -22
View File
@@ -28,13 +28,14 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.Commands;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoPositionCommand;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
import com.pathplanner.lib.commands.PathPlannerAuto;
// Subsystems
// import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Vision;
@@ -156,26 +157,6 @@ public class RobotContainer {
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
// @ /* Trim Test Buttons */
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
.onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
// ! /* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
@@ -248,8 +229,20 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
//return autoPlayback;
return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
try{
// Load the path you want to follow using its name in the GUI
return new PathPlannerAuto("New Auto");
} catch (Exception e) {
DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
return Commands.none();
}
// zach told me to do the below comment
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
// return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
}
/**
* A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+51 -4
View File
@@ -5,6 +5,7 @@
package frc4388.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.CANcoder;
@@ -30,6 +31,13 @@ import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
import edu.wpi.first.wpilibj.DriverStation;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.config.RobotConfig;
public class SwerveDrive extends Subsystem {
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
@@ -54,6 +62,45 @@ public class SwerveDrive extends Subsystem {
this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision;
RobotConfig config;
try{
config = RobotConfig.fromGUISettings();
} catch (Exception e) {
// Handle exception as needed
config = null;
}
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
return pose;//getRotation().times(-1)
}, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
.withSpeeds(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 PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
),
config, // The robot configuration
() -> {
// 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
);
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
@@ -79,8 +126,8 @@ public class SwerveDrive extends Subsystem {
if (fieldRelative) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
// double rot = 0;
@@ -118,8 +165,8 @@ public class SwerveDrive extends Subsystem {
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
}