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

Update PIDs, move things to constants.

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-14 17:32:35 -07:00
parent 882acc9b93
commit cfe3ece8e5
3 changed files with 28 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+10 -7
View File
@@ -36,10 +36,12 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder; 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.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d; 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.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N3;
@@ -150,7 +152,6 @@ public final class Constants {
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); 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 DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14);
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
} }
public static final class PIDConstants { public static final class PIDConstants {
@@ -182,13 +183,14 @@ public final class Constants {
} }
public static final class AutoConstants { public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final Gains XY_GAINS = new Gains(3,0,0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final Gains ROT_GAINS = new Gains(0.05,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 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());
} }
@@ -290,6 +292,7 @@ public final class Constants {
// public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
// Test april tag field layout
public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
Arrays.asList(new AprilTag[] { Arrays.asList(new AprilTag[] {
new AprilTag(1, new Pose3d( new AprilTag(1, new Pose3d(
src/main/java/frc4388/robot/RobotContainer.java
+3 -2
View File
@@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.GenericHID;
import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.OIConstants; 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.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -166,7 +167,7 @@ public class RobotContainer {
// ? /* Operator Buttons */ // ? /* Operator Buttons */
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) 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) new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
@@ -222,7 +223,7 @@ public class RobotContainer {
*/ */
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
//return autoPlayback; //return autoPlayback;
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.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains; 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(14.579471999999997,0.24587199999999998);
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999); // 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 xPID = new PID(AutoConstants.XY_GAINS, 0);
private PID yPID = new PID(xygains, 0); private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
private PID rotPID = new PID(rotgains, 0); private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
private Pose2d targetpos;
SwerveDrive swerveDrive; SwerveDrive swerveDrive;
Vision vision; Vision vision;
@@ -34,9 +32,10 @@ public class GotoPositionCommand extends Command {
* @param SwerveDrive m_robotSwerveDrive * @param SwerveDrive m_robotSwerveDrive
*/ */
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
this.swerveDrive = swerveDrive; this.swerveDrive = swerveDrive;
this.vision = vision; this.vision = vision;
this.targetpos = targetpos;
addRequirements(swerveDrive); addRequirements(swerveDrive);
} }
@@ -49,8 +48,6 @@ public class GotoPositionCommand extends Command {
double xerr; double xerr;
double yerr; double yerr;
double roterr; double roterr;
double xytolerance = 0.01;
double rottolerance = 1;
@Override @Override
public void execute() { public void execute() {
@@ -58,8 +55,8 @@ public class GotoPositionCommand extends Command {
yerr = targetpos.getY() - vision.getPose2d().getY(); yerr = targetpos.getY() - vision.getPose2d().getY();
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr); // SmartDashboard.putNumber("PID Y Error", yerr);
double xoutput = xPID.update(xerr); double xoutput = xPID.update(xerr);
double youtput = yPID.update(yerr); double youtput = yPID.update(yerr);
@@ -76,16 +73,16 @@ public class GotoPositionCommand extends Command {
0 0
); );
SmartDashboard.putNumber("PID X Output", xoutput); // SmartDashboard.putNumber("PID X Output", xoutput);
SmartDashboard.putNumber("PID Y Output", youtput);
// SmartDashboard.putNumber("PID Y Output", youtput); // SmartDashboard.putNumber("PID Y Output", youtput);
// // SmartDashboard.putNumber("PID Y Output", youtput);
swerveDrive.driveWithInput(leftStick, rightStick, true); swerveDrive.driveWithInput(leftStick, rightStick, true);
} }
@Override @Override
public final boolean isFinished() { 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 // this statement is a boolean in and of itself
} }