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

feeding vision into pose estimation

Browse Source
This commit is contained in:
Tylr-J42 2025-02-17 03:20:28 -05:00
parent 2e9f294cdb
commit aa6a0366e6
11 changed files with 157 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/deploy/pathplanner/autos/One Coral Left.auto
View File

@ -7,7 +7,7 @@
{
"type": "path",
"data": {
"pathName": "Start to 60 Right"
"pathName": "Start to 30 Right"
}
},
{

6
src/main/deploy/pathplanner/autos/Two Coral Left.auto
View File

@ -7,7 +7,7 @@
{
"type": "path",
"data": {
"pathName": "Start to 60 Right"
"pathName": "Start to 30 Right"
}
},
{
@ -19,7 +19,7 @@
{
"type": "path",
"data": {
"pathName": "60 Right to HP"
"pathName": "30 Right to HP"
}
},
{
@ -31,7 +31,7 @@
{
"type": "path",
"data": {
"pathName": "HP to 300 Left"
"pathName": "HP to 330 Left"
}
},
{

54
src/main/deploy/pathplanner/paths/150 Right to HP.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.289041095890411,
"y": 2.9732020547945206
},
"prevControl": null,
"nextControl": {
"x": 5.950171232876712,
"y": 1.6208904109589037
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.4424657534246574,
"y": 0.7944777397260271
},
"prevControl": {
"x": 2.389083904109589,
"y": 2.5374571917808213
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 54.162347045721745
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 120.96375653207336
},
"useDefaultConstraints": true
}

0
src/main/deploy/pathplanner/paths/60 Right to HP.path → src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

0
src/main/deploy/pathplanner/paths/300 Right to HP.path → src/main/deploy/pathplanner/paths/330 Right to HP.path
View File

0
src/main/deploy/pathplanner/paths/HP to 300 Left.path → src/main/deploy/pathplanner/paths/HP to 330 Left.path
View File

61
src/main/deploy/pathplanner/paths/Right Start to 150 Right.path Normal file
View File

@ -0,0 +1,61 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 7.602996575342465,
"y": 2.0115582191780823
},
"prevControl": null,
"nextControl": {
"x": 6.491095890410959,
"y": 1.9965325342465756
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 5.289041095890411,
"y": 2.9882277397260277
},
"prevControl": {
"x": 6.130479452054795,
"y": 2.11673801369863
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.7142857142857124,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 120.25643716352937
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 90.0
},
"useDefaultConstraints": true
}

0
src/main/deploy/pathplanner/paths/Start to 60 Right.path → src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

10
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.OIConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
@ -19,6 +20,9 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -45,13 +49,15 @@ public class RobotContainer {
private CommandXboxController operator;
private SendableChooser<Command> autoChooser;
private Vision vision;
public RobotContainer() {
climberPivot = new ClimberPivot();
climberRollers = new ClimberRollers();
drivetrain = new Drivetrain();
vision = new Vision(drivetrain::getGyroValue);
drivetrain = new Drivetrain(vision);
elevator = new Elevator();
//elevator = new ElevatorSysID();
@ -253,7 +259,7 @@ public class RobotContainer {
sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
}
}
public Command getAutonomousCommand() {
return autoChooser.getSelected();

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

@ -21,6 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
@ -35,13 +36,15 @@ public class Drivetrain extends SubsystemBase {
protected MAXSwerveModule m_rearRight;
// The gyro sensor
private AHRS ahrs;
private AHRS gyro;
// Odometry class for tracking robot pose
private SwerveDrivePoseEstimator m_estimator;
private Vision vision;
/** Creates a new DriveSubsystem. */
public Drivetrain() {
public Drivetrain(Vision vision) {
m_frontLeft = new MAXSwerveModule(
DrivetrainConstants.kFrontLeftDrivingCanId,
DrivetrainConstants.kFrontLeftTurningCanId,
@ -66,11 +69,11 @@ public class Drivetrain extends SubsystemBase {
DrivetrainConstants.kBackRightChassisAngularOffset
);
ahrs = new AHRS(NavXComType.kMXP_SPI);
gyro = new AHRS(NavXComType.kMXP_SPI);
m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(ahrs.getAngle()),
Rotation2d.fromDegrees(gyro.getAngle()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
@ -109,6 +112,24 @@ public class Drivetrain extends SubsystemBase {
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
// if the detected tags match your alliances reef tags use their pose estimates
if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
}else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
}
if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
}
}
public ChassisSpeeds getCurrentChassisSpeeds() {
@ -228,11 +249,11 @@ public class Drivetrain extends SubsystemBase {
/** Zeroes the heading of the robot. */
public void zeroHeading() {
ahrs.reset();
gyro.reset();
}
public double getGyroValue() {
return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
}
/**
@ -250,7 +271,7 @@ public class Drivetrain extends SubsystemBase {
* @return The turn rate of the robot, in degrees per second
*/
public double getTurnRate() {
return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
}
public void addVisionMeasurement(Pose2d pose, double timestamp){

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

@ -74,7 +74,6 @@ public class Vision{
}
public Pose2d getBlackGlobalPose(){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()),
new Rotation2d(gyroAngle.getAsDouble()));
@ -109,9 +108,8 @@ public class Vision{
}
public Pose2d getOrangeGlobalPose(){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()),
return relativeToGlobalPose2d(getOrangeClosestTag(),
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()),
new Rotation2d(gyroAngle.getAsDouble()));
}