diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 638bfc3..edddfc7 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -18,9 +18,8 @@ class AutoDrive(metaclass=Singleton): def __init__(self): self._toSpeaker = False self._toPickup = False - self._rfp = RepulsorFieldPlanner() + self.rfp = RepulsorFieldPlanner() self._trajCtrl = HolonomicDriveController() - self._curPose = Pose2d() self._telemTraj = [] self._obsDet = ObstacleDetector() @@ -32,35 +31,37 @@ def getTrajectory(self) -> Trajectory|None: return None # TODO def updateTelemetry(self) -> None: - self._telemTraj = self._rfp.updateTelemetry(self._curPose) + self._telemTraj = self.rfp.updateTelemetry() def getWaypoints(self) -> list[Pose2d]: return self._telemTraj def getObstacles(self) -> list[Translation2d]: - return self._rfp.getObstacleTransList() + return self.rfp.getObstacleTransList() + + def isRunning(self)->bool: + return self.rfp.goal != None def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand: retCmd = cmdIn # default - no auto driving - self._curPose = curPose for obs in self._obsDet.getObstacles(curPose): - self._rfp.add_obstcale_observaton(obs) + self.rfp.add_obstcale_observaton(obs) - self._rfp.decay_observations() + self.rfp._decay_observations() # Handle command changes if(self._toPickup): - self._rfp.setGoal(transform(GOAL_PICKUP)) + self.rfp.setGoal(transform(GOAL_PICKUP)) elif(self._toSpeaker): - self._rfp.setGoal(transform(GOAL_SPEAKER)) + self.rfp.setGoal(transform(GOAL_SPEAKER)) elif(not self._toSpeaker and not self._toPickup): - self._rfp.setGoal(None) + self.rfp.setGoal(None) # If being asked to auto-align, use the command from the dynamic path planner if(self._toPickup or self._toSpeaker): - olCmd = self._rfp.getCmd(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) log("AutoDrive FwdRev Cmd", olCmd.velX, "mps") log("AutoDrive Strafe Cmd", olCmd.velY, "mps") log("AutoDrive Rot Cmd", olCmd.velT, "radpers") diff --git a/humanInterface/ledControl.py b/humanInterface/ledControl.py new file mode 100644 index 0000000..c023316 --- /dev/null +++ b/humanInterface/ledControl.py @@ -0,0 +1,38 @@ +from wpilib import PWMMotorController +from utils.constants import LED_STACK_LIGHT_CTRL_PWM +from utils.singleton import Singleton +from wpimath.filter import Debouncer + +BLINK = -1.0 +GREEN = 0.35 +RED = 0.03 +BLUE = 0.85 +OFF = 0.0 + +class LEDControl(metaclass=Singleton): + def __init__(self): + + self._isAutoDrive = False + self._isStuck = False + self.stuckDebounce = Debouncer(0.3, Debouncer.DebounceType.kFalling) + self.ctrlBlock = PWMMotorController("LEDCtrl", LED_STACK_LIGHT_CTRL_PWM) + + def update(self): + + stuckDebounced = self.stuckDebounce.calculate(self._isStuck) + + if(self._isAutoDrive): + if(stuckDebounced): + pwmVal = RED * BLINK + else: + pwmVal = BLUE + else: + pwmVal = GREEN + + self.ctrlBlock.set(pwmVal) + + def setAutoDrive(self, isAutoDrive:bool): + self._isAutoDrive = isAutoDrive + + def setStuck(self, isStuck:bool): + self._isStuck = isStuck \ No newline at end of file diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 6c925f3..93a4051 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -1,6 +1,4 @@ -from dataclasses import dataclass -import math -from wpimath.geometry import Pose2d, Translation2d, Transform2d, Rotation2d +from wpimath.geometry import Pose2d, Translation2d from navigation.navConstants import FIELD_X_M, FIELD_Y_M from drivetrain.drivetrainCommand import DrivetrainCommand @@ -42,8 +40,12 @@ ]) # These define how far in advance we attempt to plan for telemetry purposes -TELEM_LOOKAHEAD_DIST_M = 3.0 -TELEM_LOOKAHEAD_STEPS = 7 +LOOKAHEAD_DIST_M = 3.0 +LOOKAHEAD_STEPS = 7 +LOOKAHEAD_STEP_SIZE = LOOKAHEAD_DIST_M/LOOKAHEAD_STEPS + +# If the lookahead routine's end poit is within this, we declare ourselves stuck. +STUCK_DIST = LOOKAHEAD_DIST_M/4 class RepulsorFieldPlanner: def __init__(self): @@ -53,6 +55,7 @@ def __init__(self): self.transientObstcales:list[Obstacle] = [] self.distToGo:float = 0.0 self.goal:Pose2d|None = None + self.lookaheadTraj:list[Pose2d] = [] def setGoal(self, goal:Pose2d|None): self.goal = goal @@ -70,7 +73,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force: # no goal, no force return Force() - def decay_observations(self): + def _decay_observations(self): # Linear decay of each transient obstacle observation for obs in self.transientObstcales: obs.strength -= 0.01 @@ -87,6 +90,15 @@ def getObstacleTransList(self) -> list[Translation2d]: return retArr + def isStuck(self) -> bool: + if(len(self.lookaheadTraj) > 3 and self.goal is not None): + start = self.lookaheadTraj[0] + end = self.lookaheadTraj[-1] + dist = (end-start).translation().norm() + distToGoal = (end - self.goal).translation().norm() + return dist < STUCK_DIST and distToGoal > LOOKAHEAD_DIST_M * 2 + else: + return False def atGoal(self, trans:Translation2d)->bool: if(self.goal is None): @@ -94,7 +106,7 @@ def atGoal(self, trans:Translation2d)->bool: else: return (self.goal.translation() - trans).norm() < GOAL_MARGIN_M - def getForceAtTrans(self, trans:Translation2d)->Force: + def _getForceAtTrans(self, trans:Translation2d)->Force: goalForce = self.getGoalForce(trans) @@ -112,8 +124,12 @@ def getForceAtTrans(self, trans:Translation2d)->Force: return netForce + def update(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: + curCmd = self._getCmd(curPose, stepSize_m) + self._doLookahead(curPose) + return curCmd - def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: + def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: retVal = DrivetrainCommand() # Default, no command if(self.goal is not None): @@ -126,6 +142,7 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: # Slow down when we're near the goal slowFactor = GOAL_SLOW_DOWN_MAP.lookup(self.distToGo) + nextTrans = curTrans for _ in range(4): @@ -133,7 +150,7 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: if (nextTrans - curTrans).norm() >= stepSize_m: break - netForce = self.getForceAtTrans(nextTrans) + netForce = self._getForceAtTrans(nextTrans) # Calculate a substep in the direction of the force step = Translation2d(stepSize_m*netForce.unitX()*0.5, stepSize_m*netForce.unitY()*0.5) @@ -149,23 +166,33 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: retVal.desPose = Pose2d(nextTrans, self.goal.rotation()) else: self.distToGo = 0.0 - + return retVal - def updateTelemetry(self, curPose:Pose2d) -> list[Pose2d]: - telemTraj = [] - stepsize = TELEM_LOOKAHEAD_DIST_M/TELEM_LOOKAHEAD_STEPS + def _doLookahead(self, curPose): + self.lookaheadTraj = [] if(self.goal is not None): cp = curPose - for _ in range(0,TELEM_LOOKAHEAD_STEPS): - telemTraj.append(cp) - tmp = self.getCmd(cp, stepsize) + self.lookaheadTraj.append(cp) + + for _ in range(0,LOOKAHEAD_STEPS): + tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE) if(tmp.desPose is not None): cp = tmp.desPose + self.lookaheadTraj.append(cp) + + if((cp - self.goal).translation().norm() < LOOKAHEAD_STEP_SIZE): + # At the goal, no need to keep looking ahead + break else: + # Weird, no pose given back, give up. break + def updateTelemetry(self) -> list[Pose2d]: log("PotentialField Num Obstacles", len(self.fixedObstacles) + len(self.transientObstcales)) log("PotentialField Path Active", self.goal is not None) log("PotentialField DistToGo", self.distToGo, "m") - return telemTraj + if(self.goal is not None): + return self.lookaheadTraj + else: + return [] diff --git a/robot.py b/robot.py index f80e181..0d2b54b 100644 --- a/robot.py +++ b/robot.py @@ -6,6 +6,7 @@ from drivetrain.drivetrainCommand import DrivetrainCommand from drivetrain.drivetrainControl import DrivetrainControl from humanInterface.driverInterface import DriverInterface +from humanInterface.ledControl import LEDControl from navigation.obstacles import PointObstacle from utils.segmentTimeTracker import SegmentTimeTracker from utils.signalLogging import SignalWrangler @@ -39,6 +40,7 @@ def robotInit(self): self.stt = SegmentTimeTracker() self.dInt = DriverInterface() + self.ledCtrl = LEDControl() self.autoSequencer = AutoSequencer() @@ -68,6 +70,10 @@ def robotPeriodic(self): self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints()) self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles()) + self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) + self.ledCtrl.setStuck(self.autodrive.rfp.isStuck()) + self.ledCtrl.update() + self.stt.end() ######################################################### @@ -111,22 +117,23 @@ def teleopPeriodic(self): # For test purposes, inject a series of obstacles around the current pose ct = self.driveTrain.poseEst.getCurEstPose().translation() tfs = [ - Translation2d(3.0, -0.5), - Translation2d(3.0, 0.0), - Translation2d(-3.0, 0.5), - Translation2d(-3.0, 0.0) + Translation2d(1.7, -0.5), + Translation2d(0.75, -0.75), + Translation2d(1.7, 0.5), + Translation2d(0.75, 0.75), + Translation2d(2.0, 0.0), + Translation2d(0.0, 1.0), + Translation2d(0.0, -1.0), ] for tf in tfs: obs = PointObstacle(location=(ct+tf), strength=0.7) - self.autodrive._rfp.add_obstcale_observaton(obs) + self.autodrive.rfp.add_obstcale_observaton(obs) self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup()) # No trajectory in Teleop Trajectory().setCmd(None) - - ######################################################### ## Disabled-Specific init and update def disabledPeriodic(self): diff --git a/utils/constants.py b/utils/constants.py index 0338e87..a16f322 100644 --- a/utils/constants.py +++ b/utils/constants.py @@ -64,8 +64,8 @@ # Unused = 5 # Unused = 6 # Unused = 7 -# LED_STRIPS_CTRL_PWM = 8 -LED_BLOCK_CTRL_PWM = 9 +# Unused = 8 +LED_STACK_LIGHT_CTRL_PWM = 9 ####################################################################################### diff --git a/vectorPlotter.py b/vectorPlotter.py index 2dea382..823fea6 100644 --- a/vectorPlotter.py +++ b/vectorPlotter.py @@ -204,7 +204,7 @@ def clear_vectors(self): YCount = 0.1 while YCount < 8.1: while XCount < 16.4: - force = rpp.getForceAtTrans(Translation2d(XCount, YCount)) + force = rpp._getForceAtTrans(Translation2d(XCount, YCount)) plotter.add_vector(XCount, YCount, force.x, force.y) XCount += .2 YCount += .2