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

Browse Source
This commit is contained in:
Connorppeach
2025-01-14 17:43:49 -07:00
parent b7da705c0c cfe3ece8e5
commit 54a8c32564
3 changed files with 31 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+13 -10
View File
@@ -36,10 +36,12 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
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.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -153,7 +155,6 @@ public final class Constants {
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14);
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
}
public static final class PIDConstants {
@@ -185,13 +186,14 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
public static final Gains XY_GAINS = new Gains(3,0,0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final double XY_TOLERANCE = 0.05;
public static final double ROT_TOLERANCE = 1;
public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
}
@@ -289,10 +291,11 @@ public final class Constants {
public static final String CAMERA_NAME = "Camera_Module_v1";
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
// public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
// Test april tag field layout
public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
Arrays.asList(new AprilTag[] {
new AprilTag(1, new Pose3d(
src/main/java/frc4388/robot/RobotContainer.java
+6 -4
View File
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.GenericHID;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -167,7 +168,7 @@ public class RobotContainer {
// ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
@@ -224,15 +225,16 @@ public class RobotContainer {
public Command getAutonomousCommand() {
//return autoPlayback;
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
try{
// Load the path you want to follow using its name in the GUI
return new PathPlannerAuto("Example Auto");
//try{
// // Load the path you want to follow using its name in the GUI
/ return new PathPlannerAuto("Example Auto");
} catch (Exception e) {
DriverStation.reportError("Big oops: " + 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);
}
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+12 -15
View File
@@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
@@ -17,14 +18,11 @@ public class GotoPositionCommand extends Command {
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
private Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
static Gains xygains = new Gains(3,0,0);
static Gains rotgains = new Gains(0.1,0,0.0);
static double tolerance = 0;
private PID xPID = new PID(xygains, 0);
private PID yPID = new PID(xygains, 0);
private PID rotPID = new PID(rotgains, 0);
private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive;
Vision vision;
@@ -34,9 +32,10 @@ public class GotoPositionCommand extends Command {
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
this.swerveDrive = swerveDrive;
this.vision = vision;
this.targetpos = targetpos;
addRequirements(swerveDrive);
}
@@ -49,8 +48,6 @@ public class GotoPositionCommand extends Command {
double xerr;
double yerr;
double roterr;
double xytolerance = 0.01;
double rottolerance = 1;
@Override
public void execute() {
@@ -58,8 +55,8 @@ public class GotoPositionCommand extends Command {
yerr = targetpos.getY() - vision.getPose2d().getY();
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
// SmartDashboard.putNumber("PID X Error", xerr);
// SmartDashboard.putNumber("PID Y Error", yerr);
double xoutput = xPID.update(xerr);
double youtput = yPID.update(yerr);
@@ -76,16 +73,16 @@ public class GotoPositionCommand extends Command {
0
);
SmartDashboard.putNumber("PID X Output", xoutput);
SmartDashboard.putNumber("PID Y Output", youtput);
// SmartDashboard.putNumber("PID X Output", xoutput);
// SmartDashboard.putNumber("PID Y Output", youtput);
// // SmartDashboard.putNumber("PID Y Output", youtput);
swerveDrive.driveWithInput(leftStick, rightStick, true);
}
@Override
public final boolean isFinished() {
return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance);
return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
// this statement is a boolean in and of itself
}