auton is close to working, just pid work now

vikings204 · Jan 20, 2024 · 2390f66 · 2390f66
1 parent d439265
commit 2390f66
Show file tree

Hide file tree

Showing 7 changed files with 311 additions and 22 deletions.
diff --git a/autonomous.chor b/autonomous.chor
@@ -0,0 +1,171 @@
+{
+    "version": "v0.2",
+    "robotConfiguration": {
+        "mass": 37.04398850154597,
+        "rotationalInertia": 3,
+        "motorMaxTorque": 0.18121546961325966,
+        "motorMaxVelocity": 4704,
+        "gearing": 8.14,
+        "wheelbase": 0.6667496399551944,
+        "trackWidth": 0.6667496399551944,
+        "bumperLength": 0.8000995679462333,
+        "bumperWidth": 0.8000995679462333,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": {
+        "TestingPath": {
+            "waypoints": [
+                {
+                    "x": 0,
+                    "y": 0,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 0,
+                    "y": 1,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0,
+                    "y": 0,
+                    "heading": 0,
+                    "angularVelocity": -7.600376309198775e-18,
+                    "velocityX": 9.47848126641677e-34,
+                    "velocityY": 2.3685455382964595,
+                    "timestamp": 0
+                },
+                {
+                    "x": 3.444002287789685e-36,
+                    "y": 0.16666663815137714,
+                    "heading": -2.2326890701092606e-22,
+                    "angularVelocity": -3.2118068414505653e-21,
+                    "velocityX": 4.9543256977745e-35,
+                    "velocityY": 2.3975617978327155,
+                    "timestamp": 0.0695150541279211
+                },
+                {
+                    "x": 1.80940340937385e-36,
+                    "y": 0.31818178117808316,
+                    "heading": -8.171022190596369e-22,
+                    "angularVelocity": -8.542513749172842e-21,
+                    "velocityX": -2.3514315537398912e-35,
+                    "velocityY": 2.1796018851958148,
+                    "timestamp": 0.1390301082558422
+                },
+                {
+                    "x": 5.9379803896552905e-37,
+                    "y": 0.4545454170640189,
+                    "heading": -1.0153633602375655e-21,
+                    "angularVelocity": -2.8520602469919215e-21,
+                    "velocityX": -1.7486936861866492e-35,
+                    "velocityY": 1.9616417997028441,
+                    "timestamp": 0.2085451623837633
+                },
+                {
+                    "x": 9.346108964575416e-37,
+                    "y": 0.575757541803813,
+                    "heading": -9.823771839587742e-22,
+                    "angularVelocity": 4.745184644957e-22,
+                    "velocityX": 4.902720218198206e-36,
+                    "velocityY": 1.7436816565911075,
+                    "timestamp": 0.2780602165116844
+                },
+                {
+                    "x": 2.094062831073701e-36,
+                    "y": 0.6818181533947788,
+                    "heading": -8.40480447256135e-22,
+                    "angularVelocity": 2.0412375100710908e-21,
+                    "velocityX": 1.6679149082686126e-35,
+                    "velocityY": 1.5257214846699805,
+                    "timestamp": 0.34757527063960547
+                },
+                {
+                    "x": 4.0842527693840064e-36,
+                    "y": 0.7727272506353046,
+                    "heading": -6.555813623569812e-22,
+                    "angularVelocity": 2.6598423207322688e-21,
+                    "velocityX": 2.8629625672203317e-35,
+                    "velocityY": 1.3077612954632174,
+                    "timestamp": 0.41709032476752655
+                },
+                {
+                    "x": -2.861998005146919e-37,
+                    "y": 0.8484848327243157,
+                    "heading": -4.732144278399494e-22,
+                    "angularVelocity": 2.6234163690356904e-21,
+                    "velocityX": -6.287059025950303e-35,
+                    "velocityY": 1.0898010947326966,
+                    "timestamp": 0.48660537889544764
+                },
+                {
+                    "x": -2.03857738269056e-36,
+                    "y": 0.9090908990896158,
+                    "heading": -3.0214961018999903e-22,
+                    "angularVelocity": 2.4608311899881687e-21,
+                    "velocityX": -2.520860526747833e-35,
+                    "velocityY": 0.87184088577092,
+                    "timestamp": 0.5561204330233688
+                },
+                {
+                    "x": 5.106160606722849e-38,
+                    "y": 0.9545454493020581,
+                    "heading": -1.6145441512322172e-22,
+                    "angularVelocity": 2.0239528885973894e-21,
+                    "velocityX": 3.006023625587289e-35,
+                    "velocityY": 0.6538806706357015,
+                    "timestamp": 0.6256354871512899
+                },
+                {
+                    "x": 1.7914848768309607e-36,
+                    "y": 0.984848483027861,
+                    "heading": -5.689927869393762e-23,
+                    "angularVelocity": 1.5040646843361851e-21,
+                    "velocityX": 2.5036637789133933e-35,
+                    "velocityY": 0.43592045069891716,
+                    "timestamp": 0.695150541279211
+                },
+                {
+                    "x": 2.176452252156848e-48,
+                    "y": 1,
+                    "heading": 5.357751950103021e-41,
+                    "angularVelocity": 8.185174103588329e-22,
+                    "velocityX": -2.577117806968029e-35,
+                    "velocityY": 0.21796022692088016,
+                    "timestamp": 0.7646655954071322
+                },
+                {
+                    "x": 0,
+                    "y": 1,
+                    "heading": 1.7637632388066702e-41,
+                    "angularVelocity": -5.169863684148192e-40,
+                    "velocityX": -3.141686539318957e-47,
+                    "velocityY": 5.499408435381289e-40,
+                    "timestamp": 0.8341806495350533
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        1
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "4e292adc-83c9-4a24-8725-0a82efddefef"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        }
+    }
+}
diff --git a/src/main/deploy/choreo/TestingPath.traj b/src/main/deploy/choreo/TestingPath.traj
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0,
+            "y": 0,
+            "heading": 0,
+            "angularVelocity": -7.600376309198775e-18,
+            "velocityX": 9.47848126641677e-34,
+            "velocityY": 2.3685455382964595,
+            "timestamp": 0
+        },
+        {
+            "x": 3.444002287789685e-36,
+            "y": 0.16666663815137714,
+            "heading": -2.2326890701092606e-22,
+            "angularVelocity": -3.2118068414505653e-21,
+            "velocityX": 4.9543256977745e-35,
+            "velocityY": 2.3975617978327155,
+            "timestamp": 0.0695150541279211
+        },
+        {
+            "x": 1.80940340937385e-36,
+            "y": 0.31818178117808316,
+            "heading": -8.171022190596369e-22,
+            "angularVelocity": -8.542513749172842e-21,
+            "velocityX": -2.3514315537398912e-35,
+            "velocityY": 2.1796018851958148,
+            "timestamp": 0.1390301082558422
+        },
+        {
+            "x": 5.9379803896552905e-37,
+            "y": 0.4545454170640189,
+            "heading": -1.0153633602375655e-21,
+            "angularVelocity": -2.8520602469919215e-21,
+            "velocityX": -1.7486936861866492e-35,
+            "velocityY": 1.9616417997028441,
+            "timestamp": 0.2085451623837633
+        },
+        {
+            "x": 9.346108964575416e-37,
+            "y": 0.575757541803813,
+            "heading": -9.823771839587742e-22,
+            "angularVelocity": 4.745184644957e-22,
+            "velocityX": 4.902720218198206e-36,
+            "velocityY": 1.7436816565911075,
+            "timestamp": 0.2780602165116844
+        },
+        {
+            "x": 2.094062831073701e-36,
+            "y": 0.6818181533947788,
+            "heading": -8.40480447256135e-22,
+            "angularVelocity": 2.0412375100710908e-21,
+            "velocityX": 1.6679149082686126e-35,
+            "velocityY": 1.5257214846699805,
+            "timestamp": 0.34757527063960547
+        },
+        {
+            "x": 4.0842527693840064e-36,
+            "y": 0.7727272506353046,
+            "heading": -6.555813623569812e-22,
+            "angularVelocity": 2.6598423207322688e-21,
+            "velocityX": 2.8629625672203317e-35,
+            "velocityY": 1.3077612954632174,
+            "timestamp": 0.41709032476752655
+        },
+        {
+            "x": -2.861998005146919e-37,
+            "y": 0.8484848327243157,
+            "heading": -4.732144278399494e-22,
+            "angularVelocity": 2.6234163690356904e-21,
+            "velocityX": -6.287059025950303e-35,
+            "velocityY": 1.0898010947326966,
+            "timestamp": 0.48660537889544764
+        },
+        {
+            "x": -2.03857738269056e-36,
+            "y": 0.9090908990896158,
+            "heading": -3.0214961018999903e-22,
+            "angularVelocity": 2.4608311899881687e-21,
+            "velocityX": -2.520860526747833e-35,
+            "velocityY": 0.87184088577092,
+            "timestamp": 0.5561204330233688
+        },
+        {
+            "x": 5.106160606722849e-38,
+            "y": 0.9545454493020581,
+            "heading": -1.6145441512322172e-22,
+            "angularVelocity": 2.0239528885973894e-21,
+            "velocityX": 3.006023625587289e-35,
+            "velocityY": 0.6538806706357015,
+            "timestamp": 0.6256354871512899
+        },
+        {
+            "x": 1.7914848768309607e-36,
+            "y": 0.984848483027861,
+            "heading": -5.689927869393762e-23,
+            "angularVelocity": 1.5040646843361851e-21,
+            "velocityX": 2.5036637789133933e-35,
+            "velocityY": 0.43592045069891716,
+            "timestamp": 0.695150541279211
+        },
+        {
+            "x": 2.176452252156848e-48,
+            "y": 1,
+            "heading": 5.357751950103021e-41,
+            "angularVelocity": 8.185174103588329e-22,
+            "velocityX": -2.577117806968029e-35,
+            "velocityY": 0.21796022692088016,
+            "timestamp": 0.7646655954071322
+        },
+        {
+            "x": 0,
+            "y": 1,
+            "heading": 1.7637632388066702e-41,
+            "angularVelocity": -5.169863684148192e-40,
+            "velocityX": -3.141686539318957e-47,
+            "velocityY": 5.499408435381289e-40,
+            "timestamp": 0.8341806495350533
+        }
+    ]
+}
diff --git a/src/main/java/frc/robot/AutonomousManager.java b/src/main/java/frc/robot/AutonomousManager.java
@@ -18,9 +18,13 @@
 // configured for the most efficient route while avoiding obstacles
 
 public class AutonomousManager {
-    private final SwerveSubsystem SwerveSubsystem = new SwerveSubsystem();
+    private final SwerveSubsystem SwerveSubsystem;
     private ChoreoTrajectory trajectory;
 
+    public AutonomousManager(SwerveSubsystem subsystem) {
+        SwerveSubsystem = subsystem;
+    }
+
     public Command getCommand() {
         trajectory = Choreo.getTrajectory(Constants204.Autonomous.CHOREO_PATH_FILE);
 
@@ -33,10 +37,10 @@ public Command getCommand() {
                 trajectory, // Choreo trajectory from above
                 SwerveSubsystem::getPose, // A function that returns the current field-relative pose of the robot: your
                 // wheel or vision odometry
-                new PIDController(Constants.AutoConstants.kPXController, 0.0, 0.0), // PIDController for field-relative X
+                new PIDController(Constants.AutoConstants.kPXController, 0.0, 0.005), // PIDController for field-relative X
                 // translation (input: X error in meters,
                 // output: m/s).
-                new PIDController(Constants.AutoConstants.kPYController, 0.0, 0.0), // PIDController for field-relative Y
+                new PIDController(Constants.AutoConstants.kPYController, 0.0, 0.005), // PIDController for field-relative Y
                 // translation (input: Y error in meters,
                 // output: m/s).
                 thetaController, // PID constants to correct for rotation
@@ -47,9 +51,9 @@ public Command getCommand() {
                         speeds.omegaRadiansPerSecond,
                         false),*/
                 (ChassisSpeeds speeds) -> SwerveSubsystem.drive(
-                        new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond),
+                        new Translation2d(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond), // x and y are reversed somewhere else in the code
                         speeds.omegaRadiansPerSecond,
-                        false, // robot oriented swerve
+                        true, // robot oriented swerve
                         true),
                 this::isRedAlliance, // Whether or not to mirror the path based on alliance (this assumes the path is created for the blue alliance)
                 SwerveSubsystem // The subsystem(s) to require, typically your drive subsystem only

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -1,17 +1,10 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import com.revrobotics.CANSparkBase.IdleMode;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import frc.lib.config.SwerveModuleConstants;
 
@@ -85,7 +78,7 @@ public static final class Swerve {
         /* Drive Motor PID Values */
         public static final double driveKP = 1.0;
         public static final double driveKI = 0.0;
-        public static final double driveKD = 0.0;
+        public static final double driveKD = 0.01;
         public static final double driveKFF = 0.0;
 
         /* Drive Motor Characterization Values */
@@ -166,9 +159,9 @@ public static final class AutoConstants {
         public static final double kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI;
         public static final double kMaxAngularSpeedRadiansPerSecondSquared = 2 * Math.PI;
 
-        public static final double kPXController = 1;
-        public static final double kPYController = 1;
-        public static final double kPThetaController = .3;
+        public static final double kPXController = 0.3;
+        public static final double kPYController = 0.3;
+        public static final double kPThetaController = 0.3;
 
         // Constraint for the motion profilied robot angle controller
         public static final TrapezoidProfile.Constraints kThetaControllerConstraints =

diff --git a/src/main/java/frc/robot/Constants204.java b/src/main/java/frc/robot/Constants204.java
@@ -3,7 +3,7 @@
 public final class Constants204 {
     // these two are stowaways - eventually all constants will merge
     public static final class Autonomous {
-        public static final String CHOREO_PATH_FILE = "path"; // omit file extension
+        public static final String CHOREO_PATH_FILE = "TestingPath"; // omit file extension
     }
     public static final class Controller {
         public static final int PORT = 1;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -19,8 +19,8 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-    private final AutonomousManager AutonomousManager = new AutonomousManager();
     public final SwerveSubsystem SwerveSubsystem = new SwerveSubsystem();
+    private final AutonomousManager AutonomousManager = new AutonomousManager(SwerveSubsystem);
     Gamepad CONTROLLER = new Gamepad(Constants204.Controller.PORT);
 
     /**