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

Compare commits

base: Team_2648:80b59082062f24142ec9a1d2dd41de70c607748a
Branches Tags
Team_2648:main Team_2648:faster_elevator Team_2648:maybe_fix_field_oriented Team_2648:brad_elevator_control_rework Team_2648:kraken_swerve Team_2648:2025-2-1-setup
..
compare: Team_2648:maybe_fix_field_oriented
Branches Tags
Team_2648:main Team_2648:faster_elevator Team_2648:maybe_fix_field_oriented Team_2648:brad_elevator_control_rework Team_2648:kraken_swerve Team_2648:2025-2-1-setup

1 Commits
80b5908206 ... maybe_fix_

Author SHA1 Message Date
Bradley Bickford
740b4a57b9 Something to try to fix field oriented control with 2025-03-04 12:13:30 -05:00
12 changed files with 133 additions and 150 deletions
Expand all files Collapse all files

27
.OutlineViewer/outlineviewer.json
View File

@@ -1,40 +1,13 @@
{
"Clients": {
"open": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
"client@2": {
"open": true
},
"client@4": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@2": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@3": {
"open": true
},
"shuffleboard@1": {
"open": true
},
"transitory": {
"Shuffleboard": {
"Sensors Tab": {
"open": true
},
"open": true
},
"orange_Fiducial": {
"open": true
}
}
}

19
src/main/deploy/pathplanner/autos/Field Oriented Test Auto.auto Normal file
View File

@@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Field Oriented Test Path"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

2
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

54
src/main/deploy/pathplanner/paths/Field Oriented Test 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.495389344262295,
"y": 6.788165983606557
},
"prevControl": {
"x": 3.4164959016393444,
"y": 6.80015368852459
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -90.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

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

@@ -63,7 +63,7 @@ public class RobotContainer {
drivetrain = new Drivetrain();
vision = new Vision();
vision = new Vision(drivetrain::getGyroValue);
elevator = new Elevator();
//elevator = new ElevatorSysID();
@@ -109,8 +109,8 @@ public class RobotContainer {
drivetrain.setDefaultCommand(
drivetrain.drive(
() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
() -> Math.pow(driver.getLeftY(), 3),
() -> Math.pow(driver.getLeftX(), 3),
driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
() -> true
)
@@ -143,7 +143,7 @@ public class RobotContainer {
);
driver.leftTrigger().whileTrue(
manipulator.runUntilCollected(() -> 0.75)
manipulator.runUntilCollected(() -> 0.75).andThen(manipulator.retractCommand(() -> 0.25))
);
driver.start().and(driver.back()).onTrue(
@@ -153,8 +153,6 @@ public class RobotContainer {
driver.y().whileTrue(drivetrain.zeroHeading());
driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false));
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
/*
driver.rightBumper().whileTrue(
@@ -203,28 +201,26 @@ public class RobotContainer {
)
);
operator.back().onTrue(elevator.homeCommand());
operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.start().toggleOnTrue(climberPivot.runPivot(() -> operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5)));
operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition)
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
);
operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
.alongWith(manipulator.runManipulator(() -> 0.85, false))
.alongWith(manipulator.runManipulator(() -> 0.5, false))
.until(() -> driver.a().getAsBoolean())
);
@@ -245,7 +241,6 @@ public class RobotContainer {
private void configureShuffleboard() {
ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
Shuffleboard.selectTab(OIConstants.kAutoTab);
@@ -321,17 +316,6 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose);
apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag);
apriltagTab.addDouble("Orange tx", vision::getOrangeTX);
apriltagTab.addDouble("Orange ty", vision::getOrangeTY);
apriltagTab.addDouble("Orange dist", vision::getOrangeDist);
apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
}
public Command getAutonomousCommand() {

4
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@@ -43,8 +43,8 @@ public class ElevatorConstants {
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;
public static final double kL2Position = 11;
public static final double kL3Position = 27;
public static final double kL2Position = 9;
public static final double kL3Position = 23.0;
public static final double kL4Position = 50.5;
public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 23.0;

27
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@@ -29,27 +29,26 @@ public class ManipulatorPivotConstants {
public static final double kFeedForwardG = (0.3+0.19) / 2; // calculated value 0.41
public static final double kFeedForwardV = 0.68; //calculated value 0.68
public static final double kFFGravityOffset = Units.degreesToRadians(135.0+90);
public static final double kFFGravityOffset = Units.degreesToRadians(135.0);
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
public static final double kEncoderOffset = 0.78-0.25;
public static final double kEncoderOffset = 0.780;
public static final double kStartingPosition = Units.degreesToRadians(90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);
public static final double kL1Position = Units.degreesToRadians(0.0+90);
public static final double kL2Position = Units.degreesToRadians(22.0+90);
public static final double kL3Position = Units.degreesToRadians(22.0+90);
public static final double kL4Position = Units.degreesToRadians(45.0+90);
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0+90);
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0+90);
public static final double kProcessorPosition = Units.degreesToRadians(175.0+90);
public static final double kNetPosition = Units.degreesToRadians(175.0+90);
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
public static final double kL1Position = Units.degreesToRadians(0.0);
public static final double kL2Position = Units.degreesToRadians(22.0);
public static final double kL3Position = Units.degreesToRadians(22.0);
public static final double kL4Position = Units.degreesToRadians(45.0);
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
public static final double kProcessorPosition = Units.degreesToRadians(175.0);
public static final double kNetPosition = Units.degreesToRadians(175.0);
/**The closest position to the elevator brace without hitting it */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0+90);
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0+90);
public static final double kRotationLimit = Units.degreesToRadians(175.0);
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;

3
src/main/java/frc/robot/constants/OIConstants.java
View File

@@ -4,9 +4,8 @@ public class OIConstants {
public static final int kDriverControllerPort = 0;
public static final int kOperatorControllerPort = 1;
public static final double kDriveDeadband = Math.pow(0.05, 3);
public static final double kDriveDeadband = 0.05;
public static final String kAutoTab = "Auto Tab";
public static final String kSensorsTab = "Sensors Tab";
public static final String kApriltagTab = "Apriltag Tab";
}

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

@@ -19,7 +19,6 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -46,8 +45,6 @@ public class Drivetrain extends SubsystemBase {
// Odometry class for tracking robot pose
private SwerveDrivePoseEstimator m_estimator;
private TimeInterpolatableBuffer<Double> gyroBuffer = TimeInterpolatableBuffer.createDoubleBuffer(2.0);
public Orchestra m_orchestra = new Orchestra();
private Timer musicTimer = new Timer();
@@ -147,8 +144,6 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.getPosition()
});
gyroBuffer.addSample(getGyroValue(), Timer.getTimestamp());
// if the detected tags match your alliances reef tags use their pose estimates
/*
@@ -208,7 +203,14 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_estimator.resetPose(
m_estimator.resetPosition(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
pose
);
}
@@ -356,10 +358,6 @@ public class Drivetrain extends SubsystemBase {
return Rotation2d.fromDegrees(getGyroValue()).getDegrees();
}
public TimeInterpolatableBuffer<Double> getGyroBuffer(){
return gyroBuffer;
}
/**
* Returns the turn rate of the robot.
*

10
src/main/java/frc/robot/subsystems/Elevator.java
View File

@@ -174,14 +174,6 @@ public class Elevator extends SubsystemBase {
}
public Command homeCommand(){
return run(() -> {
elevatorMotor1.setVoltage(0.5);
})
.until(() -> elevatorMotor1.getOutputCurrent() > 5)
.andThen(run(() -> encoder.setPosition(0)));
}
/**
* Moves the elevator to a target destination (setpoint).
*
@@ -216,7 +208,7 @@ public class Elevator extends SubsystemBase {
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
.andThen(runManualElevator(() -> -.5)
.andThen(runManualElevator(() -> -.1)
.until(() -> encoder.getPosition() == 0));
} else {

2
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
);
pidController.setSetpoint(0);
pidController.enableContinuousInput(0, 280);
pidController.enableContinuousInput(0, 180);
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,

83
src/main/java/frc/robot/subsystems/Vision.java
View File

@@ -2,18 +2,14 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.estimator.PoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.constants.VisionConstants;
public class Vision{
@@ -27,19 +23,18 @@ public class Vision{
private DoubleSubscriber blackFramerate;
private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist;
private DoubleSubscriber orangeRobotRelativeX;
private DoubleSubscriber orangeRobotRelativeY;
private DoubleSubscriber orangeRobotRelativeZ;
private DoubleSubscriber orangeClosestTag;
private BooleanSubscriber orangeTagDetected;
private DoubleSubscriber orangeFramerate;
private double[] orangeCamPose = {0,0,0,0,0};
private double[] blackCamPose = {0,0,0,0,0};
private DoubleSupplier gyroAngle;
public Vision(){
public Vision(DoubleSupplier gyroAngle){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
@@ -54,58 +49,33 @@ public class Vision{
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
}
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroBuffer.getSample(timestamp).get()));
Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
}
public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1],
new Rotation2d());
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
.getTranslation();
Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(orangeCamPose[0])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
return robotPose;
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer);
public Pose2d getBlackGlobalPose(){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
}
public double getBlackRelativeX(){
@@ -136,26 +106,21 @@ public class Vision{
return blackFramerate.get();
}
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer
);
}else{
return new Pose2d();
}
public Pose2d getOrangeGlobalPose(){
return relativeToGlobalPose2d(getOrangeClosestTag(),
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
}
public double getOrangeTX(){
return orange_tx.get();
public double getOrangeRelativeX(){
return orangeRobotRelativeX.get();
}
public double getOrangeTY(){
return orange_ty.get();
public double getOrangeRelativeY(){
return orangeRobotRelativeY.get();
}
public double getOrangeDist(){
return orange_dist.get();
public double getOrangeRelativeZ(){
return orangeRobotRelativeZ.get();
}
public int getOrangeClosestTag(){
@@ -163,7 +128,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orange_tx.getLastChange();
return orangeRobotRelativeX.getLastChange();
}
public boolean getOrangeTagDetected(){