From 5ee57092ed43b2fcc647a9b9a8d8a2046ae6b968 Mon Sep 17 00:00:00 2001 From: Alex <75352922+agrinmanriv0537@users.noreply.github.com> Date: Sun, 7 Jan 2024 14:39:18 -0600 Subject: [PATCH] appears to be working --- .vscode/settings.json | 2 +- .../frc/lib/util/swerve/SwerveModule.java | 6 ++-- src/main/java/frc/robot/Constants.java | 31 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 8 ++--- 4 files changed, 24 insertions(+), 23 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 1444676..2e56d0f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -34,7 +34,7 @@ "editor.defaultFormatter": "redhat.java", }, "editor.codeActionsOnSave": { - "source.organizeImports": true + "source.organizeImports": "explicit" }, "editor.formatOnSave": true, "editor.formatOnPaste": true, diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index fa72d40..9c9cfcf 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -32,7 +32,7 @@ public class SwerveModule { SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); - SparkMaxPIDController angleMotorPIDController = angleMotor.getPIDController(); + SparkMaxPIDController angleMotorPIDController; /** * Creates an instance of a Swerve Module @@ -50,13 +50,13 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants /* Angle Motor Config */ angleMotor = new CANSparkMax(constants.angleMotorID, MotorType.kBrushless); + angleMotorPIDController = angleMotor.getPIDController(); + angleEncoderBuiltIn = angleMotor.getEncoder(); configAngleMotor(); /* Drive Motor Config */ driveMotor = new TalonFX(constants.driveMotorID); configDriveMotor(); - - angleEncoderBuiltIn = angleMotor.getEncoder(); angleEncoderBuiltIn .setPositionConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution); angleEncoderBuiltIn diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 67fb944..020ab01 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -92,7 +92,12 @@ public static final class Swerve { public static final boolean driveEnableCurrentLimit = true; /* Angle Motor PID Values */ - public static final double angleKP = 0.0; + // public static final double angleKP = 0.6; + // public static final double angleKI = 0.0; + // public static final double angleKD = 12.0; + // public static final double angleKF = 0.0; + + public static final double angleKP = 0.008; public static final double angleKI = 0.0; public static final double angleKD = 0.0; public static final double angleKF = 0.0; @@ -121,7 +126,7 @@ public static final class Swerve { // public static final NeutralMode angleNeutralMode = NeutralMode.Coast; public static final IdleMode angleNeutralMode = IdleMode.kCoast; - public static final NeutralMode driveNeutralMode = NeutralMode.Brake; + public static final NeutralMode driveNeutralMode = NeutralMode.Coast; /* Motor Inverts */ public static final boolean driveMotorInvert = true; @@ -135,10 +140,10 @@ public static final class Swerve { * Front Left Module - Module 0. */ public static final class Mod0 { - public static final int driveMotorID = 50; + public static final int driveMotorID = 10; public static final int angleMotorID = 8; - public static final int canCoderID = 3; - public static final double angleOffset = 139.043; + public static final int canCoderID = 10; + public static final double angleOffset = 292.588; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -147,10 +152,10 @@ public static final class Mod0 { * Front Right Module - Module 1. PROBLEM CHILD */ public static final class Mod1 { - public static final int driveMotorID = 51; + public static final int driveMotorID = 3; public static final int angleMotorID = 52; - public static final int canCoderID = 2; - public static final double angleOffset = 118.564; + public static final int canCoderID = 1; + public static final double angleOffset = 304.014; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -159,10 +164,10 @@ public static final class Mod1 { * Back Left Module - Module 2. */ public static final class Mod2 { - public static final int driveMotorID = 4; + public static final int driveMotorID = 2; public static final int angleMotorID = 2; - public static final int canCoderID = 1; - public static final double angleOffset = 259.805; + public static final int canCoderID = 2; + public static final double angleOffset = 146.426; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } @@ -171,10 +176,10 @@ public static final class Mod2 { * Back Right Module - Module 3. */ public static final class Mod3 { - public static final int driveMotorID = 3; + public static final int driveMotorID = 6; public static final int angleMotorID = 53; public static final int canCoderID = 4; - public static final double angleOffset = 31.816; + public static final double angleOffset = 5.273; public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e20bfa..f56472b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,6 @@ package frc.robot; -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.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -15,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.TestTransform; import frc.robot.subsystems.Swerve; /** @@ -52,8 +48,8 @@ public RobotContainer() { private void configureButtonBindings() { /* Driver Buttons */ driver.y().whileTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset())); - driver.x().whileTrue(new TestTransform(s_Swerve, - new Transform2d(new Translation2d(1, 0), Rotation2d.fromDegrees(180)), 7)); + // driver.x().whileTrue(new TestTransform(s_Swerve, + // new Transform2d(new Translation2d(1, 0), Rotation2d.fromDegrees(180)), 7)); driver.a().whileTrue(new InstantCommand(() -> s_Swerve.resetInitialized())); }