From 53880c2d3773829fb6fb399ea90ed54312d3980a Mon Sep 17 00:00:00 2001 From: Anirudh S Date: Sun, 15 Sep 2024 18:07:02 -0700 Subject: [PATCH] might work --- .../constants/SwerveDriveConstants.java | 5 ++ .../frc2024/subsystems/SwerveDrive.java | 4 + .../java/team3647/frc2024/util/AutoDrive.java | 76 +++++++++++++++++++ .../team3647/lib/team9442/AutoChooser.java | 1 - 4 files changed, 85 insertions(+), 1 deletion(-) diff --git a/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java b/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java index 2786fb1..710efcc 100644 --- a/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java +++ b/src/main/java/team3647/frc2024/constants/SwerveDriveConstants.java @@ -107,6 +107,11 @@ public class SwerveDriveConstants { public static final double kCouplingGearRatio = 3.57; public static final double kWheelDiameterMeters = 0.097; // 97mm public static final double kWheelRadiusInches = 1.9; + public static final double kDriveRadius = + Math.sqrt( + TunerConstants.kBackLeftXPosInches * TunerConstants.kBackLeftXPosInches + + TunerConstants.kBackLeftYPosInches + * TunerConstants.kBackLeftYPosInches); // // divide for tick to deg public static final double kTurnMotorNativeToDeg = kTurnMotorGearRatio * 360.0; diff --git a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java index 03bcda3..e396c46 100644 --- a/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java +++ b/src/main/java/team3647/frc2024/subsystems/SwerveDrive.java @@ -194,6 +194,10 @@ public void zeroPitch() { this.pitchZero = this.getPitch(); } + public double getModulePosition(int index) { + return getModule(index).getDriveMotor().getRotorPosition().getValueAsDouble(); + } + @Override public void readPeriodicInputs() { periodicIO.roll = this.m_pigeon2.getRoll().getValue(); diff --git a/src/main/java/team3647/frc2024/util/AutoDrive.java b/src/main/java/team3647/frc2024/util/AutoDrive.java index a43e236..4fab2cc 100644 --- a/src/main/java/team3647/frc2024/util/AutoDrive.java +++ b/src/main/java/team3647/frc2024/util/AutoDrive.java @@ -19,11 +19,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.List; +import java.util.Set; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import team3647.frc2024.constants.AutoConstants; import team3647.frc2024.constants.FieldConstants; +import team3647.frc2024.constants.SwerveDriveConstants; +import team3647.frc2024.constants.TunerConstants; import team3647.frc2024.subsystems.SwerveDrive; import team3647.lib.team6328.VirtualSubsystem; @@ -329,6 +333,78 @@ public Twist2d getVelocities() { return new Twist2d(getX(), getY(), getRot()); } + public Command updateWhelRadius() { + + return new Command() { + double rotTarget = 0.0; + double initialPosFR = 0.0; + double initialPosFL = 0; + double initialPosBL = 0; + double initialPosBR = 0; + double deltaRot = 0; + + @Override + public void initialize() { + rotTarget = swerve.getOdoRot().getRadians() + Math.PI; + initialPosFR = + swerve.getModule(1).getDriveMotor().getRotorPosition().getValueAsDouble(); + initialPosFL = swerve.getModulePosition(0); + initialPosBL = swerve.getModulePosition(2); + initialPosBR = swerve.getModulePosition(3); + deltaRot = swerve.getOdoRot().getRadians(); + } + + @Override + public void execute() { + swerve.drive( + 0, 0, rotController.calculate(rotTarget - swerve.getOdoRot().getRadians())); + } + + @Override + public boolean isFinished() { + return Math.abs(rotTarget - swerve.getOdoRot().getRadians()) < 0.01; + } + + @Override + public void end(boolean interrupted) { + swerve.drive(0, 0, 0); + + var finalPosFR = swerve.getModulePosition(1); + var finalPosFL = swerve.getModulePosition(0); + var finalPosBL = swerve.getModulePosition(2); + var finalPosBR = swerve.getModulePosition(3); + deltaRot = swerve.getOdoRot().getRadians() - deltaRot; + var idealDist = deltaRot * SwerveDriveConstants.kDriveRadius; + + SmartDashboard.putNumber("drot FR", finalPosFR - initialPosFR); + SmartDashboard.putNumber("drot FL", finalPosFL - initialPosFL); + SmartDashboard.putNumber("drot BL", finalPosBL - initialPosBL); + SmartDashboard.putNumber("drot BR", finalPosBR - initialPosBR); + + var radiusFR = getwheelRadius(idealDist, finalPosFR - initialPosFR); + var radisuFL = getwheelRadius(idealDist, finalPosFL - initialPosFL); + var radisuBL = getwheelRadius(idealDist, finalPosBL - initialPosBL); + var radisuBR = getwheelRadius(idealDist, finalPosBR - initialPosBR); + + SmartDashboard.putNumber( + "kWheelRadisu", (radisuBL + radisuBR + radisuFL + radiusFR) / 4); + } + + @Override + public Set getRequirements() { + return Set.of(swerve); + } + }; + } + + public double getwheelRadius(double idealDistanceM, double realDistRot) { + // 2pir*rot/gr = d + // d = dOdoRot * driveRadius + // r=(d/2pi*rot) *gr + + return ((idealDistanceM / (2 * Math.PI * realDistRot)) * TunerConstants.kDriveGearRatio); + } + public PathPlannerPath getPathToAmp() { var pose = swerve.getOdoPose(); var endHolonomicRotation = new Rotation2d(-Math.PI / 2); diff --git a/src/main/java/team3647/lib/team9442/AutoChooser.java b/src/main/java/team3647/lib/team9442/AutoChooser.java index ef34f32..78aca8d 100644 --- a/src/main/java/team3647/lib/team9442/AutoChooser.java +++ b/src/main/java/team3647/lib/team9442/AutoChooser.java @@ -1,7 +1,6 @@ package team3647.lib.team9442; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;