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