Feature/autoinit

boards/swerveboard.json

@@ -11,22 +11,6 @@
         "vgap": 16.0,
         "titleType": 0,
         "tiles": {
-          "0,0": {
-            "size": [
-              16,
-              8
-            ],
-            "content": {
-              "_type": "Field",
-              "_source0": "network_table:///SmartDashboard/Field",
-              "_title": "Field",
-              "_glyph": 148,
-              "_showGlyph": false,
-              "Game/Game": "k2024Crescendo",
-              "Visuals/Robot Icon Size": 50.0,
-              "Visuals/Show Outside Circles": false
-            }
-          },
           "16,0": {
             "size": [
               1,
@@ -583,9 +567,9 @@
     }
   ],
   "windowGeometry": {
-    "x": -8.0,
-    "y": -8.0,
-    "width": 2576.0,
-    "height": 1568.0
+    "x": -7.199999809265137,
+    "y": -7.199999809265137,
+    "width": 1550.4000244140625,
+    "height": 926.4000244140625
   }
 }

src/launcher.py

@@ -24,8 +24,9 @@ def __init__(self):
     self.firetimer = 75
 
   def stop(self):
-    # self.motor1.set(0.0)
-    # self.motor2.set(0.0)
+    self.motor1.set(0.0)
+    self.motor2.set(0.0)
+
     self.feed.set(0.0)
     self.elevator.set(0.0)
     self.firetimer = 75  # In robot cycles

src/pyproject.toml

@@ -6,7 +6,7 @@
 [tool.robotpy]
 
 # Version of robotpy this project depends on
-robotpy_version = "2024.3.1.0"
+robotpy_version = "2024.3.2.1"
 
 # Which extra RobotPy components should be installed
 # -> equivalent to `pip install robotpy[extra1, ...]
@@ -93,4 +93,4 @@ indent-style = "space"
 # Respect magic trailing commas.
 skip-magic-trailing-comma = false
 # Automatically detect the appropriate line ending.
-line-ending = "auto"
+line-ending = "auto"

src/robot.py

@@ -46,6 +46,7 @@ def init_NT(self):
     """
     self.network_table = ntcore.NetworkTableInstance.getDefault()
     self.pose_table = self.network_table.getTable(PoseInfo.name)
+
     self.odometry_x_pub = self.pose_table.getFloatTopic(PoseInfo.odometry_x).publish()
     self.odometry_y_pub = self.pose_table.getFloatTopic(PoseInfo.odometry_y).publish()
     self.odometry_r_pub = self.pose_table.getFloatTopic(PoseInfo.odometry_r).publish()
@@ -72,6 +73,7 @@ def getInputs(self):
   def setOutputs(self):
     # Robot Pose
     odometry_pose = self.drivebase.getPose()
+
     self.odometry_x_pub.set(odometry_pose.X())
     self.odometry_y_pub.set(odometry_pose.Y())
     self.odometry_r_pub.set(odometry_pose.rotation().degrees())

src/swerve/swervesubsystem.py

@@ -80,6 +80,8 @@ def __init__(self) -> None:
 
     self.gyroCalibrated = False
 
+    self.odometerGyroOffset = Rotation2d()
+
   def getAngle(self) -> float:
     return -self.gyro.getYaw()
 
@@ -103,6 +105,8 @@ def reset_motor_positions(self):
     self.back_right.resetEncoders()
 
   def resetOdometer(self, pose: Pose2d = Pose2d()):
+    self.odometerGyroOffset = pose.rotation() - self.getRotation2d()
+
     self.odometer.resetPosition(
       self.getRotation2d(),
       [
@@ -116,14 +120,16 @@ def resetOdometer(self, pose: Pose2d = Pose2d()):
     # potentially call gyro.setAngleAdjustment to align gyro with odometry (not necessary if we only use odometry)
 
   def periodic(self) -> None:
-    self.odometer.update(
-      self.getRotation2d(),
-      (
-        self.front_left.getPosition(),
-        self.front_right.getPosition(),
-        self.back_left.getPosition(),
-        self.back_right.getPosition(),
-      ),
+    (
+      self.odometer.update(
+        self.getRotation2d(),
+        (
+          self.front_left.getPosition(),
+          self.front_right.getPosition(),
+          self.back_left.getPosition(),
+          self.back_right.getPosition(),
+        ),
+      )
     )
 
     if self.isCalibrated():
@@ -156,7 +162,8 @@ def setvelocity(self, drive: float, strafe: float, rotate: float):
         x,
         y,
         z,
-        self.getRotation2d(),
+        self.getRotation2d()
+        + self.odometerGyroOffset,  # Adding the odometry gyro offset at init to correct for the 180 degree difference
       )
     else:
       chassisSpeeds = ChassisSpeeds(