Merge remote-tracking branch 'origin/abby_elliott_repulser_field_tran…

…sient_tests' into chris_repulser_field_nav_experiment

humanInterface/driverInterface.py

@@ -25,6 +25,7 @@ def __init__(self):
         self.velTSlewRateLimiter = SlewRateLimiter(rateLimit=MAX_ROTATE_ACCEL_RAD_PER_SEC_2)
         self.navToSpeaker = False
         self.navToPickup = False
+        self.createObstacle = False
 
     def update(self):
         # value of contoller buttons
@@ -62,6 +63,7 @@ def update(self):
 
             self.navToSpeaker = self.ctrl.getBButton()
             self.navToPickup = self.ctrl.getXButton()
+            self.createObstacle = self.ctrl.getRightBumper()
 
             self.connectedFault.setNoFault()
 
@@ -74,6 +76,7 @@ def update(self):
             self.navToSpeaker = False
             self.navToPickup = False
             self.connectedFault.setFaulted()
+            self.createObstacle = False
 
 
         log("DI FwdRev Cmd", self.velXCmd, "mps")
@@ -99,3 +102,6 @@ def getNavToPickup(self):
 
     def getGyroResetCmd(self):
         return self.gyroResetCmd
+
+    def getCreateObstacle(self):
+        return self.createObstacle

navigation/repulsorFieldPlanner.py

@@ -108,9 +108,10 @@ def __init__(self):
     def setGoal(self, goal:Pose2d|None):
         self.goal = goal
 
-    def add_obstcale_observaton(self, obstacle: Obstacle):
+    def add_obstcale_observaton(self, pose:Pose2d):
+        obstacle = PointObstacle(location=Translation2d(pose.X() + 3,pose.Y()),strength=.5)
         self.transientObstcales.append(obstacle)
-
+    
     def getGoalForce(self, curLocation:Translation2d) -> Force:
         if(self.goal is not None):
             displacement = self.goal.translation() - curLocation
@@ -126,7 +127,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force:
     def decay_observations(self):
         # Linear decay of each transient obstacle observation
         for obs in self.transientObstcales:
-            obs.strength -= 0.1
+            obs.strength -= 0.01
 
         # Only keep obstacles with positive strength
         self.transientObstcales = [x for x in self.transientObstcales if x.strength > 0.0]
@@ -153,7 +154,7 @@ def getForceAtTrans(self, trans:Translation2d)->Force:
             netForce += force
 
         return netForce
-
+       
     def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
         retVal = DrivetrainCommand() # Default, no command
         if(self.goal is not None):

robot.py

@@ -1,3 +1,4 @@
+from gettext import translation
 import sys
 import wpilib
 from dashboard import Dashboard
@@ -17,6 +18,7 @@
 from webserver.webserver import Webserver
 from AutoSequencerV2.autoSequencer import AutoSequencer
 from utils.powerMonitor import PowerMonitor
+from navigation.repulsorFieldPlanner import PointObstacle, RepulsorFieldPlanner
 
 class MyRobot(wpilib.TimedRobot):
     #########################################################
@@ -100,6 +102,9 @@ def teleopPeriodic(self):
         if self.dInt.getGyroResetCmd():
             self.driveTrain.resetGyro()
 
+        if self.dInt.getCreateObstacle():
+            self.autodrive._rfp.add_obstcale_observaton(self.driveTrain.poseEst.getCurEstPose())
+
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
         self.autodrive.updateTelemetry()
         self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())