From 0e3a691a2c55a4595fd33ebd917cbd4f558b544b Mon Sep 17 00:00:00 2001
From: Qbert <frc1736student@gmail.com>
Date: Thu, 26 Sep 2024 20:58:23 -0500
Subject: [PATCH 1/2] Only the right bumper on a joystick works for this (no
 keyboard keys). It's working I think! Not necessarily dodging great, but it's
 moving.

---
 humanInterface/driverInterface.py  | 6 ++++++
 navigation/repulsorFieldPlanner.py | 9 ++++++---
 robot.py                           | 6 ++++++
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py
index 52ce066..9346fad 100644
--- a/humanInterface/driverInterface.py
+++ b/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
\ No newline at end of file
diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
index 1728041..bee3539 100644
--- a/navigation/repulsorFieldPlanner.py
+++ b/navigation/repulsorFieldPlanner.py
@@ -103,8 +103,11 @@ 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)
+        print("Obstacle is ", obstacle)
+        print(self.transientObstcales)
 
     def getGoalForce(self, curLocation:Translation2d) -> Force:
         if(self.goal is not None):
@@ -121,7 +124,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]
@@ -144,7 +147,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):
diff --git a/robot.py b/robot.py
index b4e4842..cd01757 100644
--- a/robot.py
+++ b/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,10 @@ def teleopPeriodic(self):
         if self.dInt.getGyroResetCmd():
             self.driveTrain.resetGyro()
 
+        if self.dInt.getCreateObstacle():
+            self.autodrive._rfp.add_obstcale_observaton(self.driveTrain.poseEst.getCurEstPose())
+            print("Creating obstacle")
+
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
         self.autodrive.updateTelemetry()
         self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())

From 0dc54321d577b073e17859944947ecb24269d10b Mon Sep 17 00:00:00 2001
From: Qbert <frc1736student@gmail.com>
Date: Thu, 26 Sep 2024 20:59:18 -0500
Subject: [PATCH 2/2] oops got rid of my print statements

---
 navigation/repulsorFieldPlanner.py | 4 +---
 robot.py                           | 1 -
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
index bee3539..4efb7fb 100644
--- a/navigation/repulsorFieldPlanner.py
+++ b/navigation/repulsorFieldPlanner.py
@@ -106,9 +106,7 @@ def setGoal(self, goal:Pose2d|None):
     def add_obstcale_observaton(self, pose:Pose2d):
         obstacle = PointObstacle(location=Translation2d(pose.X() + 3,pose.Y()),strength=.5)
         self.transientObstcales.append(obstacle)
-        print("Obstacle is ", obstacle)
-        print(self.transientObstcales)
-
+    
     def getGoalForce(self, curLocation:Translation2d) -> Force:
         if(self.goal is not None):
             displacement = self.goal.translation() - curLocation
diff --git a/robot.py b/robot.py
index cd01757..4480193 100644
--- a/robot.py
+++ b/robot.py
@@ -104,7 +104,6 @@ def teleopPeriodic(self):
 
         if self.dInt.getCreateObstacle():
             self.autodrive._rfp.add_obstcale_observaton(self.driveTrain.poseEst.getCurEstPose())
-            print("Creating obstacle")
 
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
         self.autodrive.updateTelemetry()