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

automatic reef alignment controls

Browse Source
This commit is contained in:
Tylr-J42 2025-02-25 03:12:59 -05:00
parent 2990b917e7
commit 52e92574c4
4 changed files with 115 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,6 +9,7 @@ import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.ManipulatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.ClimberPivot;
@ -16,11 +17,12 @@ import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Manipulator;
import java.util.function.IntSupplier;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -47,8 +49,11 @@ public class RobotContainer {
private CommandXboxController operator;
private SendableChooser<Command> autoChooser;
private Vision vision;
private IntSupplier closestTag;
public RobotContainer() {
climberPivot = new ClimberPivot();
@ -70,6 +75,8 @@ public class RobotContainer {
autoChooser = AutoBuilder.buildAutoChooser();
closestTag = drivetrain::getClosestTag;
configureButtonBindings();
//elevatorSysIDBindings();
//elevatorOnlyBindings();
@ -157,6 +164,24 @@ public class RobotContainer {
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
/*
driver.rightBumper().whileTrue(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
)
);
driver.leftBumper().whileTrue(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
)
);
*/
//Operator inputs
operator.povUp().onTrue(
safeMoveManipulator(

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

@ -57,6 +57,12 @@ public class DrivetrainConstants {
public static final boolean kGyroReversed = true;
public static final double kHeadingP = 0.0;
public static final double kXTranslationP = 0.0;
public static final double kYTranslationP = 0.0;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
// Distance between front and back wheels on robot

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

@ -1,7 +1,5 @@
package frc.robot.constants;
import java.util.HashMap;
public class VisionConstants {
// global coordinate map of all tags. index is the tag id.
@ -35,22 +33,30 @@ public class VisionConstants {
//map of coral placing setpoints based on the tag that is on the same reef face
// and the on the left or right branch of that side of the reef
// <tag_number, {left_x, left_y, right_x, right_y}>
public HashMap<Integer, double[]> blueReefMap = new HashMap<>();
public HashMap<Integer, double[]> redReefMap = new HashMap<>();
public static final double[][] reefSetpointsMap = {
{},
{},
{},
{},
{},
{},
{4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
{5.789+12.272, 3.862, 5.789+12.272, 4.194},
{5.275+12.272, 5.075, 4.991+12.272, 5.246},
{3.986+12.272, 5.24, 3.701+12.272, 5.076},
{3.183+12.272, 4.191, 3.183, 3.857},
{3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
{},
{},
{},
{},
{},
{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.789, 3.862, 5.789, 4.194},
{4.993, 2.816, 5.272, 2.996}
};
VisionConstants(){
blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806});
blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857});
blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076});
blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246});
blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194});
blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996});
blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806});
blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857});
blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076});
blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246});
blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194});
blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996});
}
}

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

@ -15,6 +15,7 @@ import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil;
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;
@ -31,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.VisionConstants;
public class Drivetrain extends SubsystemBase {
// Create MAXSwerveModules
@ -50,6 +52,10 @@ public class Drivetrain extends SubsystemBase {
public Orchestra m_orchestra = new Orchestra();
private Timer musicTimer = new Timer();
private PIDController pidHeading;
private PIDController pidTranslationX;
private PIDController pidTranslationY;
/** Creates a new DriveSubsystem. */
public Drivetrain() {
m_frontLeft = new MAXSwerveModule(
@ -90,6 +96,12 @@ public class Drivetrain extends SubsystemBase {
new Pose2d()
);
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.enableContinuousInput(-180, 180);
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
AutoBuilder.configure(
this::getPose,
this::resetOdometry,
@ -152,7 +164,7 @@ public class Drivetrain extends SubsystemBase {
}
*/
if(musicTimer.get()>8){
if(musicTimer.get()>4){
if (m_orchestra.isPlaying()) {
m_orchestra.stop();
}
@ -245,6 +257,50 @@ public class Drivetrain extends SubsystemBase {
});
}
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
return run(() -> {
drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()),
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
true);
});
}
public int getClosestTag(){
if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
int closestTag = 17;
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
for(int i = 17; i <= 22; ++i){
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
if(distance < closestTagDist){
closestTag = i;
closestTagDist = distance;
}
}
return closestTag;
}else{
int closestTag = 6;
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
for(int i = 6; i <= 11; ++i){
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
if(distance < closestTagDist){
closestTag = i;
closestTagDist = distance;
}
}
return closestTag;
}
}
/**
* Sets the wheels into an X formation to prevent movement.
*/