Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

auto align works kinda

Browse Source
This commit is contained in:
Team 2648 2025-03-11 18:59:11 -04:00
parent dd50663b9e
commit 00ecedf216
6 changed files with 91 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.OutlineViewer/outlineviewer.json
View File

@ -11,6 +11,9 @@
"client@2": {
"open": true
},
"client@3": {
"open": true
},
"client@4": {
"Publishers": {
"open": true

54
src/main/deploy/pathplanner/paths/New New Path.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.891,
"y": 5.284
},
"prevControl": {
"x": 3.891,
"y": 5.284
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -119.99999999999999
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

15
src/main/java/frc/robot/RobotContainer.java
View File

@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.events.EventTrigger;
import com.pathplanner.lib.path.EventMarker;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@ -159,7 +160,7 @@ public class RobotContainer {
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
)
);
@ -167,7 +168,7 @@ public class RobotContainer {
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
)
);
@ -231,9 +232,10 @@ public class RobotContainer {
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2).andThen(manipulator.runUntilCollected(() -> 0)));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
.andThen(Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
}
@ -314,6 +316,8 @@ public class RobotContainer {
sensorTab.addDouble("velocity", drivetrain::getVelocity);
sensorTab.addDouble("heading", drivetrain::getHeading);
//sensorTab.add("odometry", drivetrain::getPose);
@ -324,6 +328,7 @@ public class RobotContainer {
apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS());
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);

2
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -57,7 +57,7 @@ public class DrivetrainConstants {
public static final boolean kGyroReversed = true;
public static final double kHeadingP = 0.01;
public static final double kHeadingP = 0.1;
public static final double kXTranslationP = 0.5;
public static final double kYTranslationP = 0.5;

2
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -54,7 +54,7 @@ public class VisionConstants {
{3.703, 3.975, 3.982, 2.806},
{3.183, 4.191, 3.183, 3.857},
{3.986, 5.24, 3.701, 5.076},
{5.275, 5.075, 4.991, 5.246},
{5.275, 5.075, 4.891, 5.284},//4.991, 5.246},
{5.789, 3.862, 5.789, 4.194},
{4.993, 2.816, 5.272, 2.996}
};

33
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -8,17 +8,20 @@ import java.io.File;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Orchestra;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.RotationTarget;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.PoseEstimator;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@ -112,7 +115,8 @@ public class Drivetrain extends SubsystemBase {
);
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.enableContinuousInput(-180, 180);
pidHeading.setTolerance(Units.degreesToRadians(3));
pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
@ -171,11 +175,11 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
}
// if the detected tags match your alliances reef tags use their pose estimates
@ -187,8 +191,9 @@ public class Drivetrain extends SubsystemBase {
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}
Logger.recordOutput("orange pose", new Pose3d());
}
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
@ -310,16 +315,22 @@ public class Drivetrain extends SubsystemBase {
});
}
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
return run(() -> {
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1),
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
return startRun(() -> {
pidHeading.reset();
pidTranslationX.reset();
pidTranslationY.reset();
},
() -> {
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05),
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05),
pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()),
true);
Logger.recordOutput("reeeee", pidHeading.getError());
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
new Rotation2d(headingSetpoint.getAsDouble()))));
headingSetpoint.get())));
});
}
@ -409,7 +420,7 @@ public class Drivetrain extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
return m_estimator.getEstimatedPosition().getRotation().getDegrees();
}
public TimeInterpolatableBuffer<Double> getGyroBuffer(){