WIP - trying to work out errors, find the correct X values and Y valu…

…es and life.

Um life is hard. I don't know what else to say

utils/allianceTransformUtils.py

@@ -1,60 +1,67 @@
 import wpilib
 from wpilib import DriverStation
-from hal import AllianceStationID
 
 from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d
-from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d
-from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d
-from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d
-from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d
+from jormungandr.choreoTrajectory import ChoreoTrajectoryState
+
+from utils.constants import FIELD_LENGTH_FT
+from utils.units import ft2m, m2ft
 
 """
  Utilities to help transform from blue alliance to red if needed
  We went rogue and chose a coordinate system where the origin is always in the 
  bottom left on the blue alliance
 """
 class AllianceTransformUtils:
+
+    """
+    #We think the code makes sense. We just don't have the actual values and don't know where they are.
     @staticmethod
-    def transformX( self, input):
-
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
-            return 16.4592 - input
+    def transformX(in_):
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
+            return (ft2m(FIELD_LENGTH_FT) - in_)
         else:
-            return input
+            return in_
     
-    def transformY(self,input):
-        return input
+    def transformY(self,in_):
+        return in_
     
-    def Rotation2d(self,input):
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
-            return Rotation2d.fromDegrees(180).minus(input)
+    def transformRotation(self,in_):
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
+            return (Rotation2d.fromDegrees(180).__sub__.in_)
         else: 
-            return input 
+            return in_
 
-    def Translation2d(self,input):
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
-            return Translation2d(AllianceTransformUtils.transformX(input.getX()), input.getY())
+    def transformTranslation(self,in_): 
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
+            #return Translation2d(self, AllianceTransformUtils.transformX(x value), AllianceTranformUtils.transformY(y value))
+            #We need to get an x to transform it, but we don't know where to get said x from. Same with y
+            return in_
         else:
-            return input
+            return in_
     
-    def Transform2d(self,input):
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
-            trans = AllianceTransformUtils.transform(input.getTranslation())
-            rot = AllianceTransformUtils.transform(input.getRotation())
-            return Transform2d(trans, rot)
+    def transformTransform(self,in_):
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
+            #translation = Transform2d((self, AllianceTransformUtils.transformX(x value), AllianceTranformUtils.transformY(y value)))
+            #rotation = AllianceTransformUtils.transformRotation(rotation value)
+            #return Transform2d(self, translation, rotation)
+            return in_
         else:
-            return input
+            return in_
         
-    def Pose2d(self,input):
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
-            trans = AllianceTransformUtils.transform(input.getTranslation())
-            rot = AllianceTransformUtils.transform(input.getRotation())
-            return Pose2d(trans, rot)
+    def transformPose(self,in_):
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
+            #trans = AllianceTransformUtils.transformTranlation(translation value)
+            #rot = AllianceTransformUtils.transformPose(rotation)
+            #return Pose2d(trans, rot)
+            return in_
         else:
-            return input
+            return in_
+    """
+
 
     def transformChoreoTrajectoryState(self,input):
-        if wpilib._wpilib.DriverStation.Alliance == Alliance.kRed:
+        if wpilib._wpilib.DriverStation.Alliance == wpilib._wpilib.DriverStation.Alliance.kRed:
             return input.flipped()
         else:
             return input

utils/units.py

@@ -25,7 +25,7 @@ def m2ft(inVal):
 
 
 def ft2m(inVal):
-    return inVal / 3.28084
+    return float(inVal / 3.28084)
 
 
 def m2in(inVal):