diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..e66f771 Binary files /dev/null and b/.DS_Store differ diff --git a/.gradle/8.5/checksums/checksums.lock b/.gradle/8.5/checksums/checksums.lock index a9525d8..290d337 100644 Binary files a/.gradle/8.5/checksums/checksums.lock and b/.gradle/8.5/checksums/checksums.lock differ diff --git a/.gradle/8.5/executionHistory/executionHistory.bin b/.gradle/8.5/executionHistory/executionHistory.bin index 9ef8ed3..6f32194 100644 Binary files a/.gradle/8.5/executionHistory/executionHistory.bin and b/.gradle/8.5/executionHistory/executionHistory.bin differ diff --git a/.gradle/8.5/executionHistory/executionHistory.lock b/.gradle/8.5/executionHistory/executionHistory.lock index bd88c95..e4bbec7 100644 Binary files a/.gradle/8.5/executionHistory/executionHistory.lock and b/.gradle/8.5/executionHistory/executionHistory.lock differ diff --git a/.gradle/8.5/fileHashes/fileHashes.bin b/.gradle/8.5/fileHashes/fileHashes.bin index ab347e0..4a9438a 100644 Binary files a/.gradle/8.5/fileHashes/fileHashes.bin and b/.gradle/8.5/fileHashes/fileHashes.bin differ diff --git a/.gradle/8.5/fileHashes/fileHashes.lock b/.gradle/8.5/fileHashes/fileHashes.lock index 41efa25..4958b7e 100644 Binary files a/.gradle/8.5/fileHashes/fileHashes.lock and b/.gradle/8.5/fileHashes/fileHashes.lock differ diff --git a/.gradle/8.5/fileHashes/resourceHashesCache.bin b/.gradle/8.5/fileHashes/resourceHashesCache.bin index 1bf9f20..6c750c5 100644 Binary files a/.gradle/8.5/fileHashes/resourceHashesCache.bin and b/.gradle/8.5/fileHashes/resourceHashesCache.bin differ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 64e95da..3ec424a 100644 Binary files a/.gradle/buildOutputCleanup/buildOutputCleanup.lock and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 45cc764..16df8df 100644 Binary files a/.gradle/file-system.probe and b/.gradle/file-system.probe differ diff --git a/bin/main/.DS_Store b/bin/main/.DS_Store new file mode 100644 index 0000000..b740bc5 Binary files /dev/null and b/bin/main/.DS_Store differ diff --git a/bin/main/frc/.DS_Store b/bin/main/frc/.DS_Store new file mode 100644 index 0000000..22623c1 Binary files /dev/null and b/bin/main/frc/.DS_Store differ diff --git a/bin/main/frc/robot/.DS_Store b/bin/main/frc/robot/.DS_Store new file mode 100644 index 0000000..37d2bb6 Binary files /dev/null and b/bin/main/frc/robot/.DS_Store differ diff --git a/bin/main/frc/robot/commands/shooter/ShooterWindup.class b/bin/main/frc/robot/commands/shooter/ShooterWindup.class index c3dfe9a..42542f7 100644 Binary files a/bin/main/frc/robot/commands/shooter/ShooterWindup.class and b/bin/main/frc/robot/commands/shooter/ShooterWindup.class differ diff --git a/bin/main/frc/robot/subsystems/.DS_Store b/bin/main/frc/robot/subsystems/.DS_Store new file mode 100644 index 0000000..9125cd0 Binary files /dev/null and b/bin/main/frc/robot/subsystems/.DS_Store differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/.DS_Store b/bin/main/frc/robot/subsystems/drivetrain/.DS_Store new file mode 100644 index 0000000..c136e55 Binary files /dev/null and b/bin/main/frc/robot/subsystems/drivetrain/.DS_Store differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class new file mode 100644 index 0000000..f629e51 Binary files /dev/null and b/bin/main/frc/robot/subsystems/drivetrain/swerve/DriveFFController.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/Polynomial.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/Polynomial.class deleted file mode 100644 index 0b859c8..0000000 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/Polynomial.class and /dev/null differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class index fbe3482..8e03e7f 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class differ diff --git a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class index ea24481..d7b84c3 100644 Binary files a/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class and b/bin/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class differ diff --git a/build/classes/java/main/frc/robot/commands/shooter/ShooterWindup.class b/build/classes/java/main/frc/robot/commands/shooter/ShooterWindup.class index abea6f2..2b91daa 100644 Binary files a/build/classes/java/main/frc/robot/commands/shooter/ShooterWindup.class and b/build/classes/java/main/frc/robot/commands/shooter/ShooterWindup.class differ diff --git a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class index 2c84753..c183286 100644 Binary files a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class and b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$1.class differ diff --git a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$2.class b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$2.class index 1892636..7ea0870 100644 Binary files a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$2.class and b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive$2.class differ diff --git a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class index f8995b7..c280ccc 100644 Binary files a/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class and b/build/classes/java/main/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.class differ diff --git a/build/libs/2024-Crescendo-Offseason.jar b/build/libs/2024-Crescendo-Offseason.jar index efb986b..cd2e6d0 100644 Binary files a/build/libs/2024-Crescendo-Offseason.jar and b/build/libs/2024-Crescendo-Offseason.jar differ diff --git a/build/tmp/compileJava/previous-compilation-data.bin b/build/tmp/compileJava/previous-compilation-data.bin index 6294953..fad9a8f 100644 Binary files a/build/tmp/compileJava/previous-compilation-data.bin and b/build/tmp/compileJava/previous-compilation-data.bin differ diff --git a/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000..4a3a78a Binary files /dev/null and b/src/.DS_Store differ diff --git a/src/main/.DS_Store b/src/main/.DS_Store new file mode 100644 index 0000000..1b94156 Binary files /dev/null and b/src/main/.DS_Store differ diff --git a/src/main/java/.DS_Store b/src/main/java/.DS_Store new file mode 100644 index 0000000..b740bc5 Binary files /dev/null and b/src/main/java/.DS_Store differ diff --git a/src/main/java/frc/.DS_Store b/src/main/java/frc/.DS_Store new file mode 100644 index 0000000..22623c1 Binary files /dev/null and b/src/main/java/frc/.DS_Store differ diff --git a/src/main/java/frc/robot/.DS_Store b/src/main/java/frc/robot/.DS_Store new file mode 100644 index 0000000..37d2bb6 Binary files /dev/null and b/src/main/java/frc/robot/.DS_Store differ diff --git a/src/main/java/frc/robot/commands/shooter/ShooterWindup.java b/src/main/java/frc/robot/commands/shooter/ShooterWindup.java index 26981d8..25b9818 100644 --- a/src/main/java/frc/robot/commands/shooter/ShooterWindup.java +++ b/src/main/java/frc/robot/commands/shooter/ShooterWindup.java @@ -1,7 +1,6 @@ package frc.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel; public class ShooterWindup extends Command { diff --git a/src/main/java/frc/robot/subsystems/.DS_Store b/src/main/java/frc/robot/subsystems/.DS_Store new file mode 100644 index 0000000..9125cd0 Binary files /dev/null and b/src/main/java/frc/robot/subsystems/.DS_Store differ diff --git a/src/main/java/frc/robot/subsystems/drivetrain/.DS_Store b/src/main/java/frc/robot/subsystems/drivetrain/.DS_Store new file mode 100644 index 0000000..c136e55 Binary files /dev/null and b/src/main/java/frc/robot/subsystems/drivetrain/.DS_Store differ diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java new file mode 100644 index 0000000..cc8068a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java @@ -0,0 +1,123 @@ +package frc.robot.subsystems.drivetrain.swerve; + +import java.util.ArrayList; + +public class DriveFFController { + ArrayList points = new ArrayList(); + public DriveFFController() { + // drive meters per second, radians per second, mps error + points.add(new double[] {0, 0, 0}); + points.add(new double[] {1, 0, 0}); + points.add(new double[] {2, 0, 0}); + points.add(new double[] {0, Math.PI, 0}); + points.add(new double[] {0, 2 * Math.PI, 0}); + points.add(new double[] {2, 2 * Math.PI, 0.45}); + points.add(new double[] {1, 2 * Math.PI, 0.212}); + points.add(new double[] {2, Math.PI, 0.344}); + points.add(new double[] {1, Math.PI, 0.25}); + } + + public double calculate(double imps, double irps) { + double mps = Math.abs(imps); + double rps = Math.abs(irps); + for (int i = 0; i < points.size(); i++) { + if (points.get(i)[0] == mps && points.get(i)[1] == rps) { + return points.get(i)[2]; + } + } + + double[] inputPoint = new double[] {mps, rps, 0}; + + double[][] p = new double[4][3]; + for (int i = 0; i < 4; i++) { + p[i] = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, -1}; + } + + ArrayList temp = points; + for (int n = 0; n < 4; n++) { + int index = 0; + for(int i = 0; i < temp.size(); i++) { + if (distance(temp.get(i), inputPoint) < distance(p[n], inputPoint)) { + boolean good = true; + for (int m = 0; m < n; m++) { + if (distance(p[m], temp.get(i)) > Math.sqrt(2) + 0.1) { + good = false; + } + } + if (good) { + p[n] = temp.get(i); + index = i; + } + } + } + temp.remove(index); + } + + double[] tr, tl, br, bl; + tr = new double[] {-Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; + tl = new double[] {Integer.MAX_VALUE, -Integer.MAX_VALUE, 0}; + br = new double[] {-Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; + bl = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, 0}; + + boolean trT = false; + boolean tlT = false; + boolean brT = false; + boolean blT = false; + + + for (int i = 0; i < 4; i++) { + if (p[i][0] >= inputPoint[0] && p[i][1] >= inputPoint[1] && !trT) { + tr = p[i]; + trT = true; + } + else if (p[i][0] <= inputPoint[0] && p[i][1] >= inputPoint[1] && !tlT) { + tl = p[i]; + tlT = true; + } + else if (p[i][0] >= inputPoint[0] && p[i][1] <= inputPoint[1] && !brT) { + br = p[i]; + brT = true; + } + else if (p[i][0] <= inputPoint[0] && p[i][1] <= inputPoint[1] && !blT) { + bl = p[i]; + blT = true; + } + } + + double r1 = (tr[0] - inputPoint[0])/(tr[0] - tl[0]) * tl[2] + (inputPoint[0] - tl[0])/(tr[0] - tl[0]) * tr[2]; + double r2 = (br[0] - inputPoint[0])/(br[0] - bl[0]) * bl[2] + (inputPoint[0] - bl[0])/(br[0] - bl[0]) * br[2]; + double pf = (inputPoint[1] - br[1])/(tr[1] - br[1]) * r1 + (tr[1] - inputPoint[1])/(tr[1] - bl[1]) * r2; + + if (Double.isNaN(r1)) { + return r2; + } + if (Double.isNaN(r2)) { + return r1; + } + + if (mps >= 0 && rps >= 0) { + return pf; + } + + if (mps >= 0 && rps <= 0) { + return -pf; + } + + if (mps <= 0 && rps >= 0) { + return -pf; + } + + return pf; + + } + + private double distance(double[] a, double[] b) { + return Math.sqrt(Math.pow(a[0] - b[0], 2) + Math.pow((a[1] - b[1]) / Math.PI, 2)); + } + + public static void main(String[] args) { + DriveFFController driveFFController = new DriveFFController(); + + System.out.println(driveFFController.calculate(1, 1)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/Polynomial.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/Polynomial.java deleted file mode 100644 index 6c2240f..0000000 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/Polynomial.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.subsystems.drivetrain.swerve; - -public class Polynomial { - private int DEGREE; - // intercept first - private double[] COEFFICIENTS; - - public Polynomial(int DEGREE, double[] COEFFICIENTS) { - this.DEGREE = DEGREE; - this.COEFFICIENTS = COEFFICIENTS; - } - - public double function(double[] x) { - double result = COEFFICIENTS[0]; - for (int i = 1; i < COEFFICIENTS.length; i++) { - int p = i % DEGREE; - if (p == 0) { - p = DEGREE; - } - - result += COEFFICIENTS[i] * Math.pow(x[((i - 1) / DEGREE)], p); - } - - return result; - } - - // testing code - // public static void main(String[] args) { - // Polynomial poly = new Polynomial(3, new double[] {4, 1, 2, 14, 2, 5, 4}); - // System.out.println(poly.function(new double[] {6.5, 1.6})); - - // } - -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java index 5f7f100..3884e07 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java @@ -8,12 +8,14 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; 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.geometry.Rotation3d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -57,10 +59,10 @@ public class SwerveDrive extends SubsystemBase{ private ChassisSpeeds measuredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0); private ChassisSpeeds measuredFeildRelativeSpeeds = new ChassisSpeeds(0,0,0); - private PIDController m_angleFeedbackController = new PIDController(0.00, 0.0, 0.000); - private SimpleMotorFeedforward m_angleFeedForwardController = new SimpleMotorFeedforward(0, 1); + private PIDController m_angleFeedbackController; + private DriveFFController driveFFController; - private PIDController m_translationalFeedbackController = new PIDController(0.00, 0.0, 0.000); + private PIDController m_translationalFeedbackController; private SwerveModuleState[] measuredModuleStates = { new SwerveModuleState(), @@ -81,6 +83,8 @@ public SwerveDrive() { new ModuleIOSim() }; gyroIO = new GyroIO() {}; + + break; default: modules = new ModuleIO[] { @@ -182,6 +186,7 @@ public void periodic() { m_poseEstimator.getEstimatedPosition().getRotation().getRadians() }); + desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredFeildRelativeSpeeds, yaw); Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); Logger.recordOutput("SwerveDrive/measuredRobotRelativeSpeeds", measuredRobotRelativeSpeeds); Logger.recordOutput("SwerveDrive/desiredFeildRelativeSpeeds", desiredFeildRelativeSpeeds); @@ -189,9 +194,10 @@ public void periodic() { ChassisSpeeds correctedSpeeds = desiredRobotRelativeSpeeds; - correctedSpeeds.omegaRadiansPerSecond = - m_angleFeedForwardController.calculate(desiredRobotRelativeSpeeds.omegaRadiansPerSecond, Math.signum(desiredRobotRelativeSpeeds.omegaRadiansPerSecond - measuredRobotRelativeSpeeds.omegaRadiansPerSecond)) + - m_angleFeedbackController.calculate(measuredRobotRelativeSpeeds.omegaRadiansPerSecond, desiredRobotRelativeSpeeds.omegaRadiansPerSecond); + // correctedSpeeds.omegaRadiansPerSecond = + // m_angleFeedForwardController.calculate(desiredRobotRelativeSpeeds.omegaRadiansPerSecond, Math.signum(desiredRobotRelativeSpeeds.omegaRadiansPerSecond - measuredRobotRelativeSpeeds.omegaRadiansPerSecond)) + + // m_angleFeedbackController.calculate(measuredRobotRelativeSpeeds.omegaRadiansPerSecond, desiredRobotRelativeSpeeds.omegaRadiansPerSecond); + Logger.recordOutput("SwerveDrive/correctedSpeeds", correctedSpeeds); @@ -206,7 +212,6 @@ public void periodic() { public void driveFieldRelative(ChassisSpeeds speeds) { desiredFeildRelativeSpeeds = speeds; - desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, yaw); } public void driveRobotRelatve(ChassisSpeeds speeds) {