Reset works now + more various updates

chopshop-166 · Dec 31, 2023 · 9eaafce · 9eaafce
1 parent 760d958
commit 9eaafce
Show file tree

Hide file tree

Showing 7 changed files with 43 additions and 19 deletions.
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
@@ -0,0 +1,24 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.5,
+      "y": 0.5
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Valk Testing Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null
+}
diff --git a/src/main/deploy/pathplanner/autos/Valk Testing.auto b/src/main/deploy/pathplanner/autos/Valk Testing.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 0.0,
-      "y": 0.0
+      "x": 1.5,
+      "y": 0.5
     },
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "command": {
     "type": "sequential",

diff --git a/src/main/deploy/pathplanner/paths/Valk Testing Path.path b/src/main/deploy/pathplanner/paths/Valk Testing Path.path
@@ -3,24 +3,24 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.0,
-        "y": 0.0
+        "x": 1.5,
+        "y": 0.5
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.8729429755192678,
-        "y": 0.0
+        "x": 1.0877785196461234,
+        "y": 0.4937454545319242
       },
       "isLocked": false
     },
     {
       "anchor": {
-        "x": 0.5981713014108248,
-        "y": 0.0
+        "x": 0.19,
+        "y": 0.5
       },
       "prevControl": {
-        "x": 0.5935685068319716,
-        "y": 0.00475773116802268
+        "x": 0.18539720542114685,
+        "y": 0.5047577311680227
       },
       "nextControl": null,
       "isLocked": false

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -290,7 +290,7 @@ public void configureButtonBindings() {
     @Override
     public void populateDashboard() {
         Shuffleboard.getTab("AutoBuilder");
-        Shuffleboard.getTab("AutoBuilder").add("Auto 1", autoChooser);
+        Shuffleboard.getTab("AutoBuilder").add("Auto", autoChooser);
     }
 
     /**

diff --git a/src/main/java/frc/robot/Vision.java b/src/main/java/frc/robot/Vision.java
@@ -64,7 +64,7 @@ public Vision(
     }
 
     public void setPose(Pose2d pose) {
-        odometry.resetPosition(Rotation2d.fromDegrees(driveMap.gyro().getAngle() - 180), getModulePositions(), pose);
+        estimator.resetPosition(Rotation2d.fromDegrees(driveMap.gyro().getAngle() - 180), getModulePositions(), pose);
     }
 
     // Estimated pose from a combination of vision and odometry

diff --git a/src/main/java/frc/robot/maps/subsystems/SwerveDriveMap.java b/src/main/java/frc/robot/maps/subsystems/SwerveDriveMap.java
@@ -73,6 +73,7 @@ public class SwerveModuleData {
             public double[] steeringTempC;
             public SwerveModuleState desiredState = new SwerveModuleState();
             public double steeringSetpoint;
+            public ChassisSpeeds speeds;
 
             public SwerveModuleData(String name) {
                 this.name = name;
@@ -97,6 +98,8 @@ public void toLog(LogTable table) {
                 subTable.put("SteeringTempCelsius", this.steeringTempC);
 
                 subTable.put("SteeringSetpoint", this.steeringSetpoint);
+                // subTable.put("ChassisSpeeds", this.speeds);
+
             }
 
             public void fromLog(LogTable table) {
@@ -117,6 +120,7 @@ public void fromLog(LogTable table) {
                 this.steeringCurrentAmps = subTable.get("SteeringCurrentAmps", this.steeringCurrentAmps);
                 this.steeringTempC = subTable.get("SteeringTempCelsius", this.steeringTempC);
                 this.steeringSetpoint = subTable.get("SteeringSetpoint", this.steeringSetpoint);
+                // this.speeds = subTable.get("ChassisSpeeds", this.speeds);
             }
 
             public void update(SwerveModule module) {
@@ -129,6 +133,7 @@ public void update(SwerveModule module) {
                 this.steeringTempC = module.getSteeringMotor().getTemperatureC();
                 module.setDesiredState(this.desiredState);
                 this.steeringSetpoint = module.getSteeringMotor().get();
+                // this.speeds = module.ChassisSpeeds.get();
             }
 
             public SwerveModuleState getModuleStates() {

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -103,10 +103,6 @@ public Pose2d getPose() {
         return pose;
     }
 
-    public void resetPose(Pose2d pose) {
-        this.pose = pose;
-    }
-
     private Vision vision;
     private Pose2d pose = new Pose2d();
     private final DrivePID drivePID;
@@ -136,7 +132,7 @@ public Drive(SwerveDriveMap map) {
 
         AutoBuilder.configureHolonomic(
                 this::getPose, // Robot pose supplier
-                this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
+                vision::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
                 this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
                 this::move, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
                 new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your
@@ -294,7 +290,6 @@ private void move(final ChassisSpeeds speeds) {
         io.rearRight.desiredState = moduleStates[3];
 
         // All the states
-
     }
 
     public void setModuleStates(SwerveModuleState[] moduleStates) {