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()