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 ad6b0e8..8a8801d 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/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): diff --git a/robot.py b/robot.py index 4133a63..7d161a7 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,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())