def initTable(self, subtable): self.table = subtable from networktables import StringArray, NumberArray self.commands = StringArray() self.ids = NumberArray() self.toCancel = NumberArray() self.table.putValue("Names", self.commands) self.table.putValue("Ids", self.ids) self.table.putValue("Cancel", self.toCancel)
def on_next(self, key_value): key, value = key_value self.nt.putValue(key, NumberArray.from_list(value)) # Log the value to let the console see it too if not self.silent: print('NT : {} : put : {:10} : {!s}'.format(self.table_name, key, self.nt.getValue(key)))
def calculateShooterParams(armAngle): hullTable = None try: hullTable = NetworkTable.getTable("GRIP/goalConvexHulls") except KeyError: return None centerXs = NumberArray() centerYs = NumberArray() areas = NumberArray() heights = NumberArray() widths = NumberArray() try: hullTable.retrieveValue("centerX", centerXs) hullTable.retrieveValue("centerY", centerYs) hullTable.retrieveValue("area", areas) hullTable.retrieveValue("height", heights) hullTable.retrieveValue("width", widths) except KeyError: return None avgLen = (len(centerXs) + len(centerYs) + len(areas) + len(heights) + len(widths))/5 if round(avgLen) != avgLen: # It happens. I don't know why. return None contours = [] for i in range(len(centerXs)): contours.append(Contour(centerXs[i], centerYs[i], areas[i], heights[i], widths[i])) if len(contours) < 1: return None # Couldn't find any vision targets contours = sorted(contours, key=lambda x: x.area, reverse=True) # Sort contours by area in descending size largest = contours[0] # Maybe use width? dy = (config.image_height / 2) - largest.centerY # Calculations taken from TowerTracker y = largest.centerY + largest.height / 2.0 y = -((2*(y/config.image_height))-1) distance = (config.center_target_height - calculateCameraHeight(armAngle)) / \ (math.tan(math.radians(y * config.vertical_fov / 2.0 + calculateCameraAngle(armAngle)))) x = largest.centerX x = (2 * (x / config.image_width)) azimuth = (x*config.horiz_fov / 2.0) - 30 dist = distanceFromTower(largest.width) return trigShootAngle(armAngle, dist)+12, azimuth, dist, largest.centerX
def __init__(self): self.size = None self.storage = Storage() self.nt = NetworkTable.getTable('/camera') self.target = NumberArray() self.nt.putValue('target', self.target) # TODO: this makes this no longer tunable.. self.lower = np.array([self.thresh_hue_p, self.thresh_sat_p, self.thresh_val_p], dtype=np.uint8) self.upper = np.array([self.thresh_hue_n, self.thresh_sat_n, self.thresh_val_n], dtype=np.uint8)
def __init__(self, physics_controller): """ :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object to communicate simulation effects to """ if LooseVersion(__version__) < LooseVersion("2016.2.5"): raise ValueError("ERROR: must have pyfrc 2016.2.5 or greater installed!") self.physics_controller = physics_controller self.physics_controller.add_device_gyro_channel("navxmxp_spi_4_angle") self.last_cam_update = -10 self.last_cam_value = None self.moved = 0 self.last_location = self.physics_controller.get_position()[:2] self.target = NumberArray() self.nt = NetworkTable.getTable("/camera") self.nt.putValue("target", self.target)
class Scheduler(Sendable): """The Scheduler is a singleton which holds the top-level running commands. It is in charge of both calling the command's run() method and to make sure that there are no two commands with conflicting requirements running. It is fine if teams wish to take control of the Scheduler themselves, all that needs to be done is to call Scheduler.getInstance().run() often to have Commands function correctly. However, this is already done for you if you use the CommandBased Robot template. .. seealso:: :class:`.Command` """ @staticmethod def _reset(): try: del Scheduler.instance except: pass @staticmethod def getInstance(): """Returns the Scheduler, creating it if one does not exist. :returns: the Scheduler """ if not hasattr(Scheduler, "instance"): Scheduler.instance = Scheduler() return Scheduler.instance def __init__(self): """Instantiates a Scheduler. """ hal.HALReport(hal.HALUsageReporting.kResourceType_Command, hal.HALUsageReporting.kCommand_Scheduler) # Active Commands self.commandTable = collections.OrderedDict() # The set of all Subsystems self.subsystems = set() # Whether or not we are currently adding a command self.adding = False # Whether or not we are currently disabled self.disabled = False # A list of all Commands which need to be added self.additions = [] # A list of all Buttons. It is created lazily. self.buttons = [] self.runningCommandsChanged = False def add(self, command): """Adds the command to the Scheduler. This will not add the :class:`.Command` immediately, but will instead wait for the proper time in the :meth:`run` loop before doing so. The command returns immediately and does nothing if given null. Adding a :class:`.Command` to the :class:`.Scheduler` involves the Scheduler removing any Command which has shared requirements. :param command: the command to add """ if command is not None: self.additions.append(command) def addButton(self, button): """Adds a button to the Scheduler. The Scheduler will poll the button during its :meth:`run`. :param button: the button to add """ self.buttons.append(button) def _add(self, command): """Adds a command immediately to the Scheduler. This should only be called in the :meth:`run` loop. Any command with conflicting requirements will be removed, unless it is uninterruptable. Giving None does nothing. :param command: the :class:`.Command` to add """ if command is None: return # Check to make sure no adding during adding if self.adding: warnings.warn( "Can not start command from cancel method. Ignoring: %s" % command, RuntimeWarning) return # Only add if not already in if command not in self.commandTable: # Check that the requirements can be had for lock in command.getRequirements(): if (lock.getCurrentCommand() is not None and not lock.getCurrentCommand().isInterruptible()): return # Give it the requirements self.adding = True for lock in command.getRequirements(): if lock.getCurrentCommand() is not None: lock.getCurrentCommand().cancel() self.remove(lock.getCurrentCommand()) lock.setCurrentCommand(command) self.adding = False # Add it to the list self.commandTable[command] = 1 self.runningCommandsChanged = True command.startRunning() def run(self): """Runs a single iteration of the loop. This method should be called often in order to have a functioning Command system. The loop has five stages: - Poll the Buttons - Execute/Remove the Commands - Send values to SmartDashboard - Add Commands - Add Defaults """ self.runningCommandsChanged = False if self.disabled: return # Don't run when disabled # Get button input (going backwards preserves button priority) for button in reversed(self.buttons): button() # Loop through the commands for command in list(self.commandTable): if not command.run(): self.remove(command) self.runningCommandsChanged = True # Add the new things for command in self.additions: self._add(command) self.additions.clear() # Add in the defaults for lock in self.subsystems: if lock.getCurrentCommand() is None: self._add(lock.getDefaultCommand()) lock.confirmCommand() self.updateTable() def registerSubsystem(self, system): """Registers a :class:`.Subsystem` to this Scheduler, so that the Scheduler might know if a default Command needs to be run. All :class:`.Subsystem` objects should call this. :param system: the system """ if system is not None: self.subsystems.add(system) def remove(self, command): """Removes the :class:`.Command` from the Scheduler. :param command: the command to remove """ if command is None or command not in self.commandTable: return del self.commandTable[command] for reqt in command.getRequirements(): reqt.setCurrentCommand(None) command.removed() def removeAll(self): """Removes all commands """ # TODO: Confirm that this works with "uninteruptible" commands for command in self.commandTable: for reqt in command.getRequirements(): reqt.setCurrentCommand(None) command.removed() self.commandTable.clear() def disable(self): """Disable the command scheduler. """ self.disabled = True def enable(self): """Enable the command scheduler. """ self.disabled = False def getName(self): return "Scheduler" def getType(self): return "Scheduler" def initTable(self, subtable): self.table = subtable from networktables import StringArray, NumberArray self.commands = StringArray() self.ids = NumberArray() self.toCancel = NumberArray() self.table.putValue("Names", self.commands) self.table.putValue("Ids", self.ids) self.table.putValue("Cancel", self.toCancel) def updateTable(self): table = self.getTable() if table is None: return # Get the commands to cancel self.table.retrieveValue("Cancel", self.toCancel) if self.toCancel: for command in self.commandTable: if id(command) in self.toCancel: command.cancel() self.toCancel.clear() self.table.putValue("Cancel", self.toCancel) if self.runningCommandsChanged: self.commands.clear() self.ids.clear() # Set the the running commands for command in self.commandTable: self.commands.append(command.getName()) self.ids.append(id(command)) self.table.putValue("Names", self.commands) self.table.putValue("Ids", self.ids) def getSmartDashboardType(self): return "Scheduler"
class TargetFinder: # Values for the lifecam-3000 #VFOV = 34.3 # Camera's vertical field of view VFOV = 45.6 HFOV = 61 # Camera's horizontal field of view VFOV_2 = VFOV / 2.0 HFOV_2 = HFOV / 2.0 target_center = 7.66 # 7' 8in camera_height = 1.08 # 13in camera_pitch = 40.0 tcx = target_center - camera_height RED = (0, 0, 255) YELLOW = (0, 255, 255) BLUE = (255, 0, 0) MOO = (255, 255, 0) colorspace = cv2.COLOR_BGR2HSV enabled = ntproperty('/camera/enabled', False) logging_enabled = ntproperty('/camera/logging_enabled', True, writeDefault=True) min_width = ntproperty('/camera/min_width', 20) #intensity_threshold = ntproperty('/camera/intensity_threshold', 75) #target_present = ntproperty('/components/autoaim/present', False) #target_angle = ntproperty('/components/autoaim/target_angle', 0) #target_height = ntproperty('/components/autoaim/target_height', 0) # boston 2013: 30 75 188 255 16 255 # virginia 2014: ? # test image: 43 100 0 255 57 255 thresh_hue_p = ntproperty('/camera/thresholds/hue_p', 60) thresh_hue_n = ntproperty('/camera/thresholds/hue_n', 120) thresh_sat_p = ntproperty('/camera/thresholds/sat_p', 90) thresh_sat_n = ntproperty('/camera/thresholds/sat_n', 255) thresh_val_p = ntproperty('/camera/thresholds/val_p', 80) thresh_val_n = ntproperty('/camera/thresholds/val_n', 255) draw = ntproperty('/camera/draw_targets', True) draw_thresh = ntproperty('/camera/draw_thresh', False) draw_c1 = ntproperty('/camera/draw_c1', False) draw_c2 = ntproperty('/camera/draw_c2', False) draw_other = ntproperty('/camera/draw_other', False) draw_hue = ntproperty('/camera/draw_hue', False) draw_sat = ntproperty('/camera/draw_sat', False) draw_val = ntproperty('/camera/draw_val', False) def __init__(self): self.size = None self.storage = Storage() self.nt = NetworkTable.getTable('/camera') self.target = NumberArray() self.nt.putValue('target', self.target) # TODO: this makes this no longer tunable.. self.lower = np.array([self.thresh_hue_p, self.thresh_sat_p, self.thresh_val_p], dtype=np.uint8) self.upper = np.array([self.thresh_hue_n, self.thresh_sat_n, self.thresh_val_n], dtype=np.uint8) def preallocate(self, img): if self.size is None or self.size[0] != img.shape[0] or self.size[1] != img.shape[1]: h, w = img.shape[:2] self.size = (h, w) # these are preallocated so we aren't allocating all the time self.gray = np.empty((h, w, 1), dtype=np.uint8) self.thresh = np.empty((h, w, 1), dtype=np.uint8) self.out = np.empty((h, w, 3), dtype=np.uint8) self.bin = np.empty((h, w, 1), dtype=np.uint8) self.hsv = np.empty((h, w, 3), dtype=np.uint8) self.hue = np.empty((h, w, 1), dtype=np.uint8) self.sat = np.empty((h, w, 1), dtype=np.uint8) self.val = np.empty((h, w, 1), dtype=np.uint8) # for overlays self.zeros = np.zeros((h, w, 1), dtype=np.bool) self.black = np.zeros((h, w, 3), dtype=np.uint8) if True: k = 2 offset = (0,0) self.kHoleClosingIterations = 1 # originally 9 self.kMinWidth = 2 # drawing self.kThickness = 1 self.kTgtThickness = 1 # accuracy of polygon approximation self.kPolyAccuracy = 10.0 self.morphKernel = cv2.getStructuringElement(cv2.MORPH_RECT, (k,k), anchor=offset) # Copy input image to output cv2.copyMakeBorder(img, 0, 0, 0, 0, cv2.BORDER_CONSTANT, value=self.RED, dst=self.out) def threshold(self, img): cv2.cvtColor(img, self.colorspace, dst=self.hsv) cv2.inRange(self.hsv, self.lower, self.upper, dst=self.bin) if False: cv2.split(self.hsv, [self.hue, self.sat, self.val]) # Threshold each component separately # Hue cv2.threshold(self.hue, self.thresh_hue_p, 255, type=cv2.THRESH_BINARY, dst=self.bin) cv2.threshold(self.hue, self.thresh_hue_n, 255, type=cv2.THRESH_BINARY_INV, dst=self.hue) cv2.bitwise_and(self.hue, self.bin, self.hue) if self.draw_hue: # # overlay green where the hue threshold is non-zero self.out[np.dstack((self.zeros, self.hue != 0, self.zeros))] = 255 # Saturation cv2.threshold(self.sat, self.thresh_sat_p, 255, type=cv2.THRESH_BINARY, dst=self.bin) cv2.threshold(self.sat, self.thresh_sat_n, 255, type=cv2.THRESH_BINARY_INV, dst=self.sat) cv2.bitwise_and(self.sat, self.bin, self.sat) if self.draw_sat: # overlay blue where the sat threshold is non-zero self.out[np.dstack((self.sat != 0, self.zeros, self.zeros))] = 255 # Value cv2.threshold(self.val, self.thresh_val_p, 255, type=cv2.THRESH_BINARY, dst=self.bin) cv2.threshold(self.val, self.thresh_val_n, 255, type=cv2.THRESH_BINARY_INV, dst=self.val) cv2.bitwise_and(self.val, self.bin, self.val) if self.draw_val: # overlay red where the val threshold is non-zero self.out[np.dstack((self.zeros, self.zeros, self.val != 0))] = 255 # Combine the results to obtain our binary image which should for the most # part only contain pixels that we care about cv2.bitwise_and(self.hue, self.sat, self.bin) cv2.bitwise_and(self.bin, self.val, self.bin) # Fill in any gaps using binary morphology cv2.morphologyEx(self.bin, cv2.MORPH_CLOSE, self.morphKernel, dst=self.bin, iterations=self.kHoleClosingIterations) if self.draw_thresh: b = (self.bin != 0) cv2.copyMakeBorder(self.black, 0, 0, 0, 0, cv2.BORDER_CONSTANT, value=self.RED, dst=self.out) self.out[np.dstack((b, b, b))] = 255 # return self.bin #def threshold # cv2.cvtColor(img,cv2.COLOR_BGR2GRAY, dst=self.gray) # ret, _ = cv2.threshold(self.gray, self.intensity_threshold, 255, cv2.THRESH_BINARY, dst=self.thresh) def find_contours(self, img): thresh_img = self.threshold(img) _, contours, _ = cv2.findContours(thresh_img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) result = [] for cnt in contours: approx = cv2.approxPolyDP(cnt,0.01*cv2.arcLength(cnt,True),True) #print(i, len(approx)) if len(approx)>5 and len(approx)<12: _,_,w,h = cv2.boundingRect(approx) if self.draw_c1: cv2.drawContours(self.out, [approx], -1, self.MOO, 2, lineType=8) if w > h and w > self.min_width: result.append(approx) return result def find_gates(self, cnt): result = [] for gate in cnt: hull = cv2.convexHull(gate) approx = cv2.approxPolyDP(hull,0.01*cv2.arcLength(hull,True),True) if len(approx) in (4,5): result.append(approx) if self.draw_other: cv2.drawContours(self.out, [approx], -1, self.YELLOW, 2, lineType=8) if self.draw_c2: cv2.drawContours(self.out, [approx], -1, self.BLUE, 2, lineType=8) return result def quad_normals(self, img): self.result = result = [] if not self.enabled: self.target_present = False return img now = time.time() if self.logging_enabled: self.storage.set_image(img) self.preallocate(img) cnt = self.find_contours(img) gates = self.find_gates(cnt) h, w = img.shape[:2] h = float(h) w = float(w) for q in gates: #M = cv2.moments(q) #cx = int(M['m10']/M['m00']) #cy = int(M['m01']/M['m00']) ((cx, cy), (rw, rh), rotation) = cv2.minAreaRect(q) gate = {} gate['d'] = q gate['av'] = self.VFOV * cy / h - self.VFOV_2 gate['ah'] = self.HFOV * cx / w - self.HFOV_2 # experimental distance value.. doesn't work gate['ad'] = (self.tcx)/math.tan(math.radians(-gate['av'] + self.camera_pitch)) result.append(gate) self.target.clear() # sort the returned data, tend to prefer the 'closest' gate to the center if len(result): result.sort(key=lambda k: abs(k['ah'])) target = result[0] if self.draw: cv2.drawContours(self.out, [target['d']], -1, self.RED, 2, lineType=8) # angle, height, ts self.target += [target['ah'], target['av'], target['ad'], now] self.nt.putValue('target', self.target) return self.out
class Scheduler(Sendable): """The Scheduler is a singleton which holds the top-level running commands. It is in charge of both calling the command's run() method and to make sure that there are no two commands with conflicting requirements running. It is fine if teams wish to take control of the Scheduler themselves, all that needs to be done is to call Scheduler.getInstance().run() often to have Commands function correctly. However, this is already done for you if you use the CommandBased Robot template. .. seealso:: :class:`.Command` """ @staticmethod def _reset(): try: del Scheduler.instance except: pass @staticmethod def getInstance(): """Returns the Scheduler, creating it if one does not exist. :returns: the Scheduler """ if not hasattr(Scheduler, "instance"): Scheduler.instance = Scheduler() return Scheduler.instance def __init__(self): """Instantiates a Scheduler. """ hal.HALReport(hal.HALUsageReporting.kResourceType_Command, hal.HALUsageReporting.kCommand_Scheduler) # Active Commands self.commandTable = collections.OrderedDict() # The set of all Subsystems self.subsystems = set() # Whether or not we are currently adding a command self.adding = False # Whether or not we are currently disabled self.disabled = False # A list of all Commands which need to be added self.additions = [] # A list of all Buttons. It is created lazily. self.buttons = [] self.runningCommandsChanged = False def add(self, command): """Adds the command to the Scheduler. This will not add the :class:`.Command` immediately, but will instead wait for the proper time in the :meth:`run` loop before doing so. The command returns immediately and does nothing if given null. Adding a :class:`.Command` to the :class:`.Scheduler` involves the Scheduler removing any Command which has shared requirements. :param command: the command to add """ if command is not None: self.additions.append(command) def addButton(self, button): """Adds a button to the Scheduler. The Scheduler will poll the button during its :meth:`run`. :param button: the button to add """ self.buttons.append(button) def _add(self, command): """Adds a command immediately to the Scheduler. This should only be called in the :meth:`run` loop. Any command with conflicting requirements will be removed, unless it is uninterruptable. Giving None does nothing. :param command: the :class:`.Command` to add """ if command is None: return # Check to make sure no adding during adding if self.adding: warnings.warn("Can not start command from cancel method. Ignoring: %s" % command, RuntimeWarning) return # Only add if not already in if command not in self.commandTable: # Check that the requirements can be had for lock in command.getRequirements(): if (lock.getCurrentCommand() is not None and not lock.getCurrentCommand().isInterruptible()): return # Give it the requirements self.adding = True for lock in command.getRequirements(): if lock.getCurrentCommand() is not None: lock.getCurrentCommand().cancel() self.remove(lock.getCurrentCommand()) lock.setCurrentCommand(command) self.adding = False # Add it to the list self.commandTable[command] = 1 self.runningCommandsChanged = True command.startRunning() def run(self): """Runs a single iteration of the loop. This method should be called often in order to have a functioning Command system. The loop has five stages: - Poll the Buttons - Execute/Remove the Commands - Send values to SmartDashboard - Add Commands - Add Defaults """ self.runningCommandsChanged = False if self.disabled: return # Don't run when disabled # Get button input (going backwards preserves button priority) for button in reversed(self.buttons): button() # Loop through the commands for command in list(self.commandTable): if not command.run(): self.remove(command) self.runningCommandsChanged = True # Add the new things for command in self.additions: self._add(command) self.additions.clear() # Add in the defaults for lock in self.subsystems: if lock.getCurrentCommand() is None: self._add(lock.getDefaultCommand()) lock.confirmCommand() self.updateTable() def registerSubsystem(self, system): """Registers a :class:`.Subsystem` to this Scheduler, so that the Scheduler might know if a default Command needs to be run. All :class:`.Subsystem` objects should call this. :param system: the system """ if system is not None: self.subsystems.add(system) def remove(self, command): """Removes the :class:`.Command` from the Scheduler. :param command: the command to remove """ if command is None or command not in self.commandTable: return del self.commandTable[command] for reqt in command.getRequirements(): reqt.setCurrentCommand(None) command.removed() def removeAll(self): """Removes all commands """ # TODO: Confirm that this works with "uninteruptible" commands for command in self.commandTable: for reqt in command.getRequirements(): reqt.setCurrentCommand(None) command.removed() self.commandTable.clear() def disable(self): """Disable the command scheduler. """ self.disabled = True def enable(self): """Enable the command scheduler. """ self.disabled = False def getName(self): return "Scheduler" def getType(self): return "Scheduler" def initTable(self, subtable): self.table = subtable from networktables import StringArray, NumberArray self.commands = StringArray() self.ids = NumberArray() self.toCancel = NumberArray() self.table.putValue("Names", self.commands) self.table.putValue("Ids", self.ids) self.table.putValue("Cancel", self.toCancel) def updateTable(self): table = self.getTable() if table is None: return # Get the commands to cancel self.table.retrieveValue("Cancel", self.toCancel) if self.toCancel: for command in self.commandTable: if id(command) in self.toCancel: command.cancel() self.toCancel.clear() self.table.putValue("Cancel", self.toCancel) if self.runningCommandsChanged: self.commands.clear() self.ids.clear() # Set the the running commands for command in self.commandTable: self.commands.add(command.getName()) self.ids.add(id(command)) self.table.putValue("Names", self.commands) self.table.putValue("Ids", self.ids) def getSmartDashboardType(self): return "Scheduler"
class PhysicsEngine(object): """ Simulates a motor moving something that strikes two limit switches, one on each end of the track. Obviously, this is not particularly realistic, but it's good enough to illustrate the point """ # Transmit data to robot via NetworkTables camera_enabled = ntproperty("/camera/enabled", False, False) ticks_per_ft = ntproperty("/components/distance_ctrl/ticks_per_ft", 0, False) camera_update_rate = 1 / 15.0 target_location = (0, 16) def __init__(self, physics_controller): """ :param physics_controller: `pyfrc.physics.core.PhysicsInterface` object to communicate simulation effects to """ if LooseVersion(__version__) < LooseVersion("2016.2.5"): raise ValueError("ERROR: must have pyfrc 2016.2.5 or greater installed!") self.physics_controller = physics_controller self.physics_controller.add_device_gyro_channel("navxmxp_spi_4_angle") self.last_cam_update = -10 self.last_cam_value = None self.moved = 0 self.last_location = self.physics_controller.get_position()[:2] self.target = NumberArray() self.nt = NetworkTable.getTable("/camera") self.nt.putValue("target", self.target) def update_sim(self, hal_data, now, tm_diff): """ Called when the simulation parameters for the program need to be updated. :param now: The current time as a float :param tm_diff: The amount of time that has passed since the last time that this function was called """ # Simulate the drivetrain lf_motor = hal_data["CAN"][4]["value"] / -1024 lr_motor = hal_data["CAN"][5]["value"] / -1024 rf_motor = hal_data["CAN"][2]["value"] / -1024 rr_motor = hal_data["CAN"][3]["value"] / -1024 speed, rotation = drivetrains.four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor, speed=8) self.physics_controller.drive(speed, rotation, tm_diff) # Simulate the firing mechanism, max is around 8000 in one second pitcher = hal_data["CAN"][7] max_v = 8000 * tm_diff vel = pitcher["enc_velocity"] if pitcher["mode_select"] == wpilib.CANTalon.ControlMode.Speed: # when in pid mode, converge to the correct value err = pitcher["value"] - vel aerr = abs(err) max_v = max(min(max_v, aerr), -aerr) vel += math.copysign(max_v, err) else: # Otherwise increment linearly vel += max_v * pitcher["value"] pitcher["enc_velocity"] = max(min(vel, 8000), -8000) x, y, angle = self.physics_controller.get_position() # encoder simulation lx, ly = self.last_location dx = x - lx dy = y - ly length = math.hypot(dx, dy) direction = math.atan2(dy, dx) error = angle - direction if abs(error) > math.pi: if error > 0: error = error - math.pi * 2 else: error = error + math.pi * 2 if abs(error) > math.pi / 2.0: length = length * -1 self.moved += length hal_data["encoder"][0]["count"] = int(self.moved * self.ticks_per_ft) * 4 self.last_location = x, y # Simulate the camera approaching the tower # -> this is a very simple approximation, should be good enough # -> calculation updated at 15hz self.target.clear() # simulate latency by delaying camera output if self.last_cam_value is not None: self.target += self.last_cam_value if self.camera_enabled: if now - self.last_cam_update > self.camera_update_rate: tx, ty = self.target_location dx = tx - x dy = ty - y distance = math.hypot(dx, dy) if distance > 6 and distance < 17: # determine the absolute angle target_angle = math.atan2(dy, dx) angle = ((angle + math.pi) % (math.pi * 2)) - math.pi # Calculate the offset, if its within 30 degrees then # the robot can 'see' it offset = math.degrees(target_angle - angle) if abs(offset) < 30: # target 'height' is a number between -18 and 18, where # the value is related to the distance away. -11 is ideal. self.last_cam_value = [offset, -(-(distance * 3) + 30), distance, now] self.last_cam_update = now else: self.last_cam_value = None self.nt.putValue("target", self.target)
def SendNumArray(table, key, arr): if isinstance(arr, np.ndarray): arr = arr.flatten().tolist() mail = NumberArray.from_list(arr) table.putValue(key, mail)