Dynamic auto new (#27)

Co-authored-by: ihernandez3647 <[email protected]> Co-authored-by: gvaldez7206 <[email protected]> Co-authored-by: Alex <[email protected]> Co-authored-by: Owen <[email protected]>
Frc5572 · Jan 28, 2023 · 3c932d1 · 3c932d1
1 parent 71c8c98
commit 3c932d1
Show file tree

Hide file tree

Showing 21 changed files with 1,949 additions and 416 deletions.
diff --git a/shuffleboard/shuffleboard.json b/shuffleboard/shuffleboard.json
diff --git a/src/main/java/frc/lib/util/FieldConstants.java b/src/main/java/frc/lib/util/FieldConstants.java
diff --git a/src/main/java/frc/robot/CTREConfigs.java → ...n/java/frc/lib/util/ctre/CTREConfigs.java b/src/main/java/frc/robot/CTREConfigs.java → ...n/java/frc/lib/util/ctre/CTREConfigs.java
@@ -1,11 +1,12 @@
-package frc.robot;
+package frc.lib.util.ctre;
 
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
 import com.ctre.phoenix.sensors.SensorTimeBase;
+import frc.robot.Constants;
 
 /**
  * CTRE config constants

diff --git a/...in/java/frc/lib/util/CTREModuleState.java → ...va/frc/lib/util/ctre/CTREModuleState.java b/...in/java/frc/lib/util/CTREModuleState.java → ...va/frc/lib/util/ctre/CTREModuleState.java
@@ -1,4 +1,4 @@
-package frc.lib.util;
+package frc.lib.util.ctre;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;

diff --git a/src/main/java/frc/robot/SwerveModule.java → ...ava/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java → ...ava/frc/lib/util/swerve/SwerveModule.java
@@ -1,4 +1,4 @@
-package frc.robot;
+package frc.lib.util.swerve;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
@@ -9,7 +9,9 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import frc.lib.math.Conversions;
-import frc.lib.util.CTREModuleState;
+import frc.lib.util.ctre.CTREModuleState;
+import frc.robot.Constants;
+import frc.robot.Robot;
 
 /**
  * Base Swerve Module Class. Creates an instance of the swerve module
@@ -30,7 +32,7 @@ public class SwerveModule {
      * @param moduleNumber Swerve Module ID. Must be unique
      * @param constants Constants specific to the swerve module
      */
-    public SwerveModule(int moduleNumber, frc.lib.util.SwerveModuleConstants constants) {
+    public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants constants) {
         this.moduleNumber = moduleNumber;
         angleOffset = constants.angleOffset;
 

diff --git a/...a/frc/lib/util/SwerveModuleConstants.java → ...ib/util/swerve/SwerveModuleConstants.java b/...a/frc/lib/util/SwerveModuleConstants.java → ...ib/util/swerve/SwerveModuleConstants.java
@@ -1,4 +1,4 @@
-package frc.lib.util;
+package frc.lib.util.swerve;
 
 /**
  * Constants file used when creating swerve modules

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -1,10 +1,13 @@
 package frc.robot;
 
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
-import frc.lib.util.SwerveModuleConstants;
+import frc.lib.util.swerve.SwerveModuleConstants;
 
 /**
  * Constants file.
@@ -13,6 +16,40 @@
 public final class Constants {
     public static final double stickDeadband = 0.1;
 
+    /**
+     * PID constants for Swerve Auto Holonomic Drive Controller
+     */
+    public static class SwerveTransformPID {
+        public static final double pidXkP = 1.5;
+        public static final double pidXkI = 0.0;
+        public static final double pidXkD = 0.0;
+        public static final double pidYkP = 1.5;
+        public static final double pidYkI = 0.0;
+        public static final double pidYkD = 0.0;
+        public static final double pidTkP = 3.0;
+        public static final double pidTkI = 0.0;
+        public static final double pidTkD = 0.0;
+
+        public static final double maxAngularVelocity = 3.0;
+        public static final double maxAngularAcceleration = 3.0;
+        public static final double stdDevMod = 2.0;
+    }
+
+    /**
+     * Camera offset constants
+     */
+    public static class CameraConstants {
+
+        public static final double pitch = 0 * Math.PI / 180;
+        public static final double roll = Math.PI;
+        public static final double yaw = 0.0;
+        public static final Transform3d kCameraToRobot =
+            new Transform3d(new Translation3d(Units.inchesToMeters(-10), Units.inchesToMeters(2),
+                Units.inchesToMeters(7.5)), new Rotation3d(roll, pitch, yaw));
+        public static final String cameraName = "pv2";
+        public static final double largestDistance = 0.1;
+    }
+
     /**
      * Swerve ID's
      */
@@ -22,9 +59,9 @@ public static final class Swerve {
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
 
         /* Drivetrain Constants */
-        public static final double trackWidth = Units.inchesToMeters(27);
-        public static final double wheelBase = Units.inchesToMeters(27);
-        public static final double wheelDiameter = Units.inchesToMeters(3.94);
+        public static final double trackWidth = Units.inchesToMeters(14);
+        public static final double wheelBase = Units.inchesToMeters(14);
+        public static final double wheelDiameter = Units.inchesToMeters(4);
         public static final double wheelCircumference = wheelDiameter * Math.PI;
 
         public static final boolean isFieldRelative = true;
@@ -33,7 +70,7 @@ public static final class Swerve {
         public static final double openLoopRamp = 0.25;
         public static final double closedLoopRamp = 0.4;
 
-        public static final double driveGearRatio = (8.14 / 1.0); // 6.86:1
+        public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1
         public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1
 
         public static final SwerveDriveKinematics swerveKinematics =
@@ -94,19 +131,19 @@ public static final class Mod0 {
             public static final int driveMotorID = 50;
             public static final int angleMotorID = 8;
             public static final int canCoderID = 3;
-            public static final double angleOffset = 138.867;
+            public static final double angleOffset = 139.043;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }
 
         /**
-         * Front Right Module - Module 1.
+         * Front Right Module - Module 1. PROBLEM CHILD
          */
         public static final class Mod1 {
             public static final int driveMotorID = 51;
             public static final int angleMotorID = 52;
             public static final int canCoderID = 2;
-            public static final double angleOffset = 109.775;
+            public static final double angleOffset = 118.564;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }
@@ -118,7 +155,7 @@ public static final class Mod2 {
             public static final int driveMotorID = 4;
             public static final int angleMotorID = 2;
             public static final int canCoderID = 1;
-            public static final double angleOffset = 259.541;
+            public static final double angleOffset = 259.805;
             public static final SwerveModuleConstants constants =
                 new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, angleOffset);
         }

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.lib.util.ctre.CTREConfigs;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,9 @@
 
 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;
@@ -12,6 +15,7 @@
 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;
 
 /**
@@ -48,6 +52,9 @@ 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.a().whileTrue(new InstantCommand(() -> s_Swerve.resetInitialized()));
     }
 
     /**

diff --git a/src/main/java/frc/robot/commands/ExecUltrasonic.java b/src/main/java/frc/robot/commands/ExecUltrasonic.java
diff --git a/src/main/java/frc/robot/commands/LimelightAlign.java b/src/main/java/frc/robot/commands/LimelightAlign.java
diff --git a/src/main/java/frc/robot/commands/MoveNewMotor.java b/src/main/java/frc/robot/commands/MoveNewMotor.java
diff --git a/src/main/java/frc/robot/commands/SpinMotor.java b/src/main/java/frc/robot/commands/SpinMotor.java