def _write(self):
"""Internal method that actually writes the table to a file. This is
called in its own thread when :func:`save` is called.
"""
with self.lock:
with self.fileLock:
self.fileLock.notify_all()
with open(self.FILE_NAME, "w") as output:
output.write("[Preferences]\n")
for key in self.keylist:
value = self.values.get(key, "")
comment = self.comments.get(key, "")
if comment:
output.write(comment)
output.write(key)
output.write(self.VALUE_PREFIX)
output.write(value)
output.write(self.VALUE_SUFFIX)
output.write(self.endComment)
try:
from networktables import NetworkTable
NetworkTable.getTable(self.TABLE_NAME).putBoolean(
self.SAVE_FIELD, False)
except ImportError:
pass
def open(self):
logger.info("NetworkTables websocket opened")
self.ioloop = IOLoop.current()
self.nt = NetworkTable.getGlobalTable()
NetworkTable.addGlobalListener(self.on_nt_change, immediateNotify=True)
self.nt.addConnectionListener(self, immediateNotify=True)
def _write(self):
"""Internal method that actually writes the table to a file. This is
called in its own thread when :func:`save` is called.
"""
with self.lock:
with self.fileLock:
self.fileLock.notify_all()
with open(self.FILE_NAME, "w") as output:
output.write("[Preferences]\n")
for key in self.keylist:
value = self.values.get(key, "")
comment = self.comments.get(key, "")
if comment:
output.write(comment)
output.write(key)
output.write(self.VALUE_PREFIX)
output.write(value)
output.write(self.VALUE_SUFFIX)
output.write(self.endComment)
try:
from networktables import NetworkTable
NetworkTable.getTable(self.TABLE_NAME).putBoolean(self.SAVE_FIELD, False)
except ImportError:
pass
def __init__(self):
"""initializes all values to presets or None if need to be set
"""
NetworkTable.initialize(server='10.44.80.2') #Current Roborio address
NetworkTable.setUpdateRate(.02)
self.numberPublish = NetworkTable.getTable('/GRIP/myContoursReport')
self.__hsl_threshold_hue = [56.6546762589928, 83.28925638990128]
self.__hsl_threshold_saturation = [229.31654676258992, 255.0]
self.__hsl_threshold_luminance = [
13.758992805755396, 51.51952461799658
]
self.hsl_threshold_output = None
self.__find_contours_input = self.hsl_threshold_output
self.__find_contours_external_only = False
self.find_contours_output = None
self.__filter_contours_contours = self.find_contours_output
self.__filter_contours_min_area = 125.0
self.__filter_contours_min_perimeter = 0.0
self.__filter_contours_min_width = 0.0
self.__filter_contours_max_width = 1000.0
self.__filter_contours_min_height = 0.0
self.__filter_contours_max_height = 1000.0
self.__filter_contours_solidity = [0, 100]
self.__filter_contours_max_vertices = 1000000.0
self.__filter_contours_min_vertices = 0.0
self.__filter_contours_min_ratio = 0.0
self.__filter_contours_max_ratio = 1000.0
self.filter_contours_output = None
def sync():
for object_, tunable in _tunables:
object_name = type(object_).__name__
default = getattr(object_, tunable)
setattr(object_, tunable,
NetworkTable.getTable(object_name).getValue(tunable, default))
for object_, printable in _printables:
object_name = type(object_).__name__
if type(printable) is tuple:
assert callable(printable[1])
try:
out = printable[1]()
except BaseException as e:
out = str(e)
printable_name = printable[0]
else:
try:
out = getattr(object_, printable)
except BaseException as e:
out = str(e)
printable_name = printable
try:
NetworkTable.getTable(object_name).putValue(printable_name, out)
except (ValueError, TypeError): # invalid type
try:
NetworkTable.getTable(object_name).putValue(
printable_name, repr(out))
except ValueError as e: # oh shit we crashed twice
log.error(repr(e))
def extra_processing(pipeline: GripPipeline):
"""
Performs extra processing on the pipeline's outputs and publishes data to NetworkTables.
:param pipeline: the pipeline that just processed an image
:return: None
"""
# TODO: Users need to implement this.
# Useful for converting OpenCV objects (e.g. contours) to something
# NetworkTables can understand.
converter = Angles()
(imgx, imgy) = pipeline.get_mat_info_size
contours = pipeline.filter_contours_output
center_x_positions = []
center_y_positions = []
widths = []
heights = []
x_angles = []
y_angles = []
dist = 0.0
# sorts contours
if (len(pipeline.filter_contours_output) > 1):
boundingBoxes = [
cv2.boundingRect(c) for c in pipeline.filter_contours_output
]
(contours, boundingBoxes) = zip(*sorted(zip(contours, boundingBoxes),
key=lambda b: b[1][1],
reverse=False))
dummy, distY, dummy2, distH = cv2.boundingRect(contours[0])
dist = converter.dist(distY + distH / 2)
for contour in contours:
x, y, w, h = cv2.boundingRect(contour)
# X and Y are coordinates of the top-left corner of the bounding box
center_x_positions.append(x + w / 2)
center_y_positions.append(y + h / 2)
widths.append(w)
heights.append(y)
x_angles.append(converter.x_angle(x + w / 2))
y_angles.append(converter.y_angle(y + h / 2))
extra = NetworkTable.getTable('/GRIP/preprocessed')
extra.putNumberArray('x', center_x_positions)
extra.putNumberArray('y', center_y_positions)
extra.putNumberArray('width', widths)
extra.putNumberArray('height', heights)
usable = NetworkTable.getTable('/GRIP/postprocessed')
usable.putNumberArray('x angles', x_angles)
usable.putNumberArray('y angles', y_angles)
usable.putNumber('distance', dist)
class TCDash(App):
NetworkTable.setIPAddress(
"127.0.0.1") #for robot will be 10.2.17.2 aka ip of rio
NetworkTable.setClientMode()
NetworkTable.initialize()
sd = NetworkTable.getTable("SmartDashboard")
def build(self):
return main()
def __init__(self, update_callback):
"""
:param update_callback: A callable with signature ```callable(update)``` for processing outgoing updates
formatted as strings.
"""
self.update_callback = update_callback
self.nt = NetworkTable.getGlobalTable()
NetworkTable.addGlobalListener(self._nt_on_change,
immediateNotify=True)
self.nt.addConnectionListener(self._nt_connected, immediateNotify=True)
def __init__(self):
NetworkTable.initialize()
self.tf = TargetFinder.TargetFinder()
self.tf.enabled = True
cv2.namedWindow('img')
for s in self.settings:
self._create_trackbar(s)
def __init__(self, fakerobot):
try:
if fakerobot:
NetworkTable.setIPAddress("localhost")
else:
NetworkTable.setIPAddress("roboRIO-4915-FRC")
NetworkTable.setClientMode()
NetworkTable.initialize()
self.sd = NetworkTable.getTable("SmartDashboard")
self.visTable = self.sd.getSubTable("Vision")
self.connectionListener = ConnectionListener()
self.visTable.addConnectionListener(self.connectionListener)
self.visTable.addTableListener(self.visValueChanged)
self.targetState = targetState.TargetState(self.visTable)
self.targetHigh = True
self.autoAimEnabled = False
self.imuHeading = 0
self.fpsHistory = []
self.lastUpdate = time.time()
except:
xcpt = sys.exc_info()
print("ERROR initializing network tables", xcpt[0])
traceback.print_tb(xcpt[2])
def __init__(self, receiverIP):
try:
# IPAddress can be static ip ("10.49.15.2" or name:"roboRIO-4915-FRC"/"localhost")
NetworkTable.setUpdateRate(.01) # default is .05 (50ms/20Hz), .01 (10ms/100Hz)
NetworkTable.setIPAddress(receiverIP)
NetworkTable.setClientMode()
NetworkTable.initialize()
self.sd = NetworkTable.getTable("SmartDashboard")
# we communcate target to robot via VisionTarget table
self.targetTable = self.sd.getSubTable("VisionTarget")
# robot communicates to us via fields within the VisionControl SubTable
# we opt for a different table to ensure we to receive callbacks from our
# own writes.
self.controlTable = self.sd.getSubTable("VisionControl")
self.controlTable.addConnectionListener(self.connectionListener)
self.controlTable.addTableListener(self.visionControlEvent)
self.control = Control()
self.target = Target()
self.fpsHistory = []
self.lastUpdate = time.time()
theComm = self
except:
xcpt = sys.exc_info()
print("ERROR initializing network tables", xcpt[0])
traceback.print_tb(xcpt[2])
def __init__(self, fakerobot):
try:
if fakerobot:
NetworkTable.setIPAddress("localhost")
else:
NetworkTable.setIPAddress("roboRIO-4915-FRC")
NetworkTable.setClientMode()
NetworkTable.initialize()
self.sd = NetworkTable.getTable("SmartDashboard")
self.visTable = self.sd.getSubTable("Vision")
self.connectionListener = ConnectionListener()
self.visTable.addConnectionListener(self.connectionListener)
self.visTable.addTableListener(self.visValueChanged)
self.targetState = targetState.TargetState(self.visTable)
self.targetHigh = True
self.autoAimEnabled = False
self.imuHeading = 0
self.fpsHistory = []
self.lastUpdate = time.time()
except:
xcpt = sys.exc_info()
print("ERROR initializing network tables", xcpt[0])
traceback.print_tb(xcpt[2])
def main():
NetworkTable.setTeam('0000') # TODO set your team number
NetworkTable.initialize()
cap = cv2.VideoCapture(0)
pipeline = GripPipeline()
while True:
ret, frame = cap.read()
if ret:
pipeline.process(
frame
) # TODO add extra parameters if the pipeline takes more than just a single image
extra_processing(pipeline)
def main():
NetworkTable.setTeam(5587) # TODO set your team number
NetworkTable.setClientMode()
# TODO switch to RIO IP, or IP of laptop running OutlineViewer for setup
NetworkTable.setIPAddress('10.55.87.2')
NetworkTable.initialize()
# TODO find what v4l2-ctl settings you need. Pass each commandline option
# through this array. REQUIRES v4l2-utils to be installed.
call([
"v4l2-ctl", "--set-ctrl=exposure_auto=1",
"--set-ctrl=exposure_absolute=9",
"--set-ctrl=white_balance_temperature_auto=0"
],
shell=False)
cap = cv2.VideoCapture(0)
pipeline = GripPipeline()
while True:
ret, frame = cap.read()
if ret:
# TODO add extra parameters if the pipeline takes more than just a
# single image
pipeline.process(frame)
extra_processing(pipeline)
def __init__(self, ip, name):
NetworkTable.setIPAddress(ip)
NetworkTable.setClientMode()
NetworkTable.initialize()
self.visionNetworkTable = NetworkTable.getTable(name)
if constants.SENDTOSMARTDASHBOARD:
constants.smartDashboard = NetworkTable.getTable("SmartDashboard")
def __init__(self):
NetworkTable.initialize()
from vision import ImageProcessor
self.tf = ImageProcessor()
self.tf.enabled = True
self.tf.tuning = True
cv2.namedWindow('img')
for s in self.settings:
self._create_trackbar(s)
cv2.createTrackbar('draw_thresh', 'img', 0, 1, lambda v: self._en_thresh(v))
def getTable(cls):
if cls.table is None:
from networktables import NetworkTable
cls.table = NetworkTable.getTable("SmartDashboard")
hal.report(hal.UsageReporting.kResourceType_SmartDashboard,
hal.UsageReporting.kSmartDashboard_Instance)
return cls.table
def on_enable(self):
"""
Constructor.
:param robotDrive: a `wpilib.RobotDrive` object
:type rf_encoder: DriveEncoders()
:type lf_encoder: DriveEncoders()
"""
# Hack for one-time initialization because magicbot doesn't support it
if not self.enabled:
nt = NetworkTable.getTable('components/autoaim')
nt.addTableListener(self._align_angle_updated, True,
'target_angle')
self.isTheRobotBackwards = False
self.iErr = 0
# set defaults here
self.y = 0
self.rotation = 0
self.squaredInputs = False
self.halfRotation = 1
self.gyro_enabled = True
self.align_angle = None
def run_server(serial_conn):
sd = NetworkTable.getTable("SmartDashboard")
def value_changed(table, key, value, is_new):
debug("value_changed(): key='%s', value=%s, is_new=%s" % (key, value, is_new))
if key in receiveList and receiveList[key] == value:
debug("value_changed(): update was caused by receive")
del receiveList[key]
else:
if isinstance(value, numbers.Number):
try:
send_int(serial_conn, key, round(value))
except OverflowError:
info("Integer too big to be sent: key=%s, value=%s" % (key, value));
elif isinstance(value, bool):
send_bool(serial_conn, key, value)
sd.addTableListener(value_changed)
for name, value in recieve_packets(serial_conn):
receiveList[name] = value
if isinstance(value, numbers.Number):
sd.putNumber(name, value)
elif isinstance(value, bool):
sd.putBoolean(name, value)
def __init__(self, sensors, forkLift, drive):
'''
:param sensors: Sensors object
:type sensors: :class:`.Sensor`
'''
self.sensors = sensors
self.forkLift = forkLift
self.drive = drive
sd = NetworkTable.getTable('SmartDashboard')
self.rotate_constant = sd.getAutoUpdateValue('Align|Rotation Constant',
0.015)
self.fwd_constant = sd.getAutoUpdateValue('Align|FwdConstant', 175.0)
self.drive_speed = sd.getAutoUpdateValue('Align|Speed', -.3)
#self.threshold = sd.getAutoUpdateValue('Align|DistThreshold', 3)
self.strafe_speed = sd.getAutoUpdateValue('Align|StrafeSpeed', .2)
# in centimeters
self.sensor_spacing = 18
self.next_pos = None
self.aligning = False
self.aligned = False
def on_enable(self):
"""
Constructor.
:param robotDrive: a `wpilib.RobotDrive` object
:type rf_encoder: DriveEncoders()
:type lf_encoder: DriveEncoders()
"""
# Hack for one-time initialization because magicbot doesn't support it
if not self.enabled:
nt = NetworkTable.getTable('components/autoaim')
nt.addTableListener(self._align_angle_updated, True, 'target_angle')
self.isTheRobotBackwards = False
self.iErr = 0
# set defaults here
self.y = 0
self.rotation = 0
self.squaredInputs = False
self.halfRotation = 1
self.gyro_enabled = True
self.align_angle = None
def __init__(self, sensors, forkLift, drive, autolift):
"""
:param sensors: Sensors object
:type sensors: :class:`.Sensor`
"""
self.sensors = sensors
self.forkLift = forkLift
self.drive = drive
self.autolift = autolift
sd = NetworkTable.getTable("SmartDashboard")
self.rotate_constant = sd.getAutoUpdateValue("Align|Rotation Constant", 0.015)
self.fwd_constant = sd.getAutoUpdateValue("Align|FwdConstant", 175.0)
self.drive_speed = sd.getAutoUpdateValue("Align|Speed", -0.3)
# self.threshold = sd.getAutoUpdateValue('Align|DistThreshold', 3)
self.strafe_speed = sd.getAutoUpdateValue("Align|StrafeSpeed", 0.2)
self.binPos = sd.getAutoUpdateValue("binPosition", 0)
# in centimeters
self.sensor_spacing = 18
self.next_pos = None
self.aligning = False
self.aligned = False
self.can_aligning = True
self.can_aligned = False
def __init__(self, webcam_device, webcam_name, local_ip="127.0.0.1", local_port=8087, calc="peg"):
self.__calc = calc
# start web cam and set dimensions
self.__camera = cv2.VideoCapture(webcam_device)
self.__camera.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.__camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
# setup cv source to write frames to for mjpeg server
self.__cvsource = cs.CvSource(webcam_name, cs.VideoMode.PixelFormat.kMJPEG, 320, 240, 30)
# setup mjpeg server
self.__mjpeg_server = cs.MjpegServer("httpserver", local_port)
self.__mjpeg_server.setSource(self.__cvsource);
# setup grip pipeline from grip.py
self.__grip_pipeline = GripPipeline()
# write camera info
table = NetworkTable.getTable("CameraPublisher")
cam = table.getSubTable(webcam_name)
cam.putString("source", "usb:IMAQdx/")
cam.putStringArray("streams", ["mjpg:http://" + local_ip + ":" + str(local_port) + "/stream.mjpg?name="+webcam_name])
if self.__debug:
self.__webcam_name = webcam_name
cv2.namedWindow(self.__webcam_name)
def __init__(self, tote_motor, can_motor):
self.sd = NetworkTable.getTable('SmartDashboard')
self.toteLimitLSensor = wpilib.DigitalInput(0) ##Left limit switch
self.toteLimitRSensor = wpilib.DigitalInput(1) ##Right limit switch
self.longDistanceLSensor = SharpIR2Y0A02(1) # # Robot's left
self.longDistanceRSensor = SharpIR2Y0A02(3) # # Robot's right
self.shortDistanceLSensor = SharpIRGP2Y0A41SK0F(2) # # Robot's left
self.shortDistanceRSensor = SharpIRGP2Y0A41SK0F(7) # # Robot's right
self.leftSensor = CombinedSensor(self.longDistanceLSensor, 22, self.shortDistanceLSensor, 6)
self.rightSensor = CombinedSensor(self.longDistanceRSensor, 22, self.shortDistanceRSensor, 6)
self.tote_motor = tote_motor
self.can_motor = can_motor
self.in_range = False
self.in_range_start = None
# Premature optimization, but it looks nicer
self._tote_exclude_range = set()
# measured using the calibration routine
interference = [(1031, 1387), (1888, 2153), (4544, 4895), (5395, 5664), (8008, 8450)]
#for i in [1033, 2031, 4554, 5393, 7902]:
for r1, r2 in interference:
for j in range(r1, r2):
self._tote_exclude_range.add(j)
self.update()
def getTable():
if SmartDashboard.table is None:
from networktables import NetworkTable
SmartDashboard.table = NetworkTable.getTable("SmartDashboard")
hal.HALReport(hal.HALUsageReporting.kResourceType_SmartDashboard,
hal.HALUsageReporting.kSmartDashboard_Instance)
return SmartDashboard.table
def __init__(self, robotDrive, gyro, backInfrared):
'''
Constructor.
:param robotDrive: a `wpilib.RobotDrive` object
'''
self.isTheRobotBackwards = False
# set defaults here
self.x = 0
self.y = 0
self.rotation = 0
self.gyro = gyro
self.angle_constant = .040
self.gyro_enabled = True
self.robotDrive = robotDrive
# Strafe stuff
self.backInfrared = backInfrared
sd = NetworkTable.getTable('SmartDashboard')
self.strafe_back_speed = sd.getAutoUpdateValue('strafe_back', .23)
self.strafe_fwd_speed = sd.getAutoUpdateValue('strafe_fwd', -.23)
# Top range: 50 is slow
# Low range: 10 is too much acceleration
self.strafe_adj = sd.getAutoUpdateValue('strafe_adj', 35)
def __init__(self, tote_motor):
self.sd = NetworkTable.getTable('SmartDashboard')
self.toteLimitLSensor = wpilib.DigitalInput(0) ##Left limit switch
self.toteLimitRSensor = wpilib.DigitalInput(1) ##Right limit switch
self.longDistanceLSensor = SharpIR2Y0A02(1) # # Robot's left
self.longDistanceRSensor = SharpIR2Y0A02(3) # # Robot's right
self.shortDistanceLSensor = SharpIRGP2Y0A41SK0F(2) # # Robot's left
self.shortDistanceRSensor = SharpIRGP2Y0A41SK0F(7) # # Robot's right
self.leftSensor = CombinedSensor(self.longDistanceLSensor, 22, self.shortDistanceLSensor, 6)
self.rightSensor = CombinedSensor(self.longDistanceRSensor, 22, self.shortDistanceRSensor, 6)
self.tote_motor = tote_motor
self.in_range = False
self.in_range_start = None
# Premature optimization, but it looks nicer
self._tote_exclude_range = set()
# measured using the calibration routine
interference = [(1031, 1387), (1888, 2153), (4544, 4895), (5395, 5664), (8008, 8450)]
#for i in [1033, 2031, 4554, 5393, 7902]:
for r1, r2 in interference:
for j in range(r1, r2):
self._tote_exclude_range.add(j)
self.update()
class test:
ip = 'roborio-2643-frc.local'
ip2 = '10.26.43.20'
NetworkTable.setIPAddress(ip2)
NetworkTable.setClientMode()
NetworkTable.initialize()
table = NetworkTable.getTable("Vision")
x = 0
def sendVal(val):
self.table.putNumber("Hello", val)
while 1:
table.putNumber("Hello", x)
print(table.getNumber("Hello", 0))
x += 1
def createObjects(self):
self.logger = logging.getLogger("robot")
self.sd = NetworkTable.getTable('SmartDashboard')
self.intake_motor = wpilib.CANTalon(14)
self.shooter_motor = wpilib.CANTalon(12)
self.defeater_motor = wpilib.CANTalon(1)
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
# needs to be created here so we can pass it in to the PIDController
self.bno055 = BNO055()
self.vision = Vision()
self.range_finder = RangeFinder(0)
self.heading_hold_pid_output = BlankPIDOutput()
Tu = 1.6
Ku = 0.6
Kp = Ku * 0.3
self.heading_hold_pid = wpilib.PIDController(0.6,
2.0 * Kp / Tu * 0.1,
1.0 * Kp * Tu / 20.0 * 0,
self.bno055, self.heading_hold_pid_output)
self.heading_hold_pid.setTolerance(3.0*math.pi/180.0)
self.heading_hold_pid.setContinuous(True)
self.heading_hold_pid.setInputRange(-math.pi, math.pi)
self.heading_hold_pid.setOutputRange(-1.0, 1.0)
self.intake_motor.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.intake_motor.reverseSensor(False)
def initializeLiveWindowComponents():
"""Initialize all the LiveWindow elements the first time we enter
LiveWindow mode. By holding off creating the NetworkTable entries, it
allows them to be redefined before the first time in LiveWindow mode.
This allows default sensor and actuator values to be created that are
replaced with the custom names from users calling addActuator and
addSensor.
"""
logger.info("Initializing the components first time")
LiveWindow.livewindowTable = NetworkTable.getTable("LiveWindow")
LiveWindow.statusTable = LiveWindow.livewindowTable.getSubTable(
"~STATUS~")
for component, c in LiveWindow.components.items():
logger.info("Initializing table for '%s' '%s'" %
(c.subsystem, c.name))
LiveWindow.livewindowTable.getSubTable(c.subsystem).putString(
"~TYPE~", "LW Subsystem")
table = LiveWindow.livewindowTable.getSubTable(
c.subsystem).getSubTable(c.name)
table.putString("~TYPE~", component.getSmartDashboardType())
table.putString("Name", c.name)
table.putString("Subsystem", c.subsystem)
component.initTable(table)
if c.isSensor:
LiveWindow.sensors.add(component)
def __init__(self, table, field):
from networktables import NetworkTable
if isinstance(table, NetworkTable):
self.table = table
else:
self.table = NetworkTable.getTable(table)
self.field = field
def getTable():
if SmartDashboard.table is None:
from networktables import NetworkTable
SmartDashboard.table = NetworkTable.getTable("SmartDashboard")
hal.HALReport(hal.HALUsageReporting.kResourceType_SmartDashboard,
hal.HALUsageReporting.kSmartDashboard_Instance)
return SmartDashboard.table
def __init__(self, robot):
# Joysticks
self.leftStick = wpilib.Joystick(JoystickMap.PORT_LEFT)
self.rightStick = wpilib.Joystick(JoystickMap.PORT_RIGHT)
self.controller = wpilib.Joystick(JoystickMap.PORT_CONTROLLER)
self.smart_dashboard = NetworkTable.getTable("SmartDashboard")
def __init__(self, table, field):
from networktables import NetworkTable
if isinstance(table, NetworkTable):
self.table = table
else:
self.table = NetworkTable.getTable(table)
self.field = field
def __init__(self, update_callback):
"""
:param update_callback: A callable with signature ```callable(update)``` for processing outgoing updates
formatted as strings.
"""
self.update_callback = update_callback
self.nt = NetworkTable.getGlobalTable()
NetworkTable.addGlobalListener(self._nt_on_change, immediateNotify=True)
class Empty:
pass
self.conn_listener = Empty()
self.conn_listener.connected = self._nt_connected
self.conn_listener.disconnected = self._nt_disconnected
self.nt.addConnectionListener(self.conn_listener, immediateNotify=True)
def setup(self):
self.left = 0
self.right = 0
self.sd = NetworkTable.getTable("/SmartDashboard")
#Turn to angle PI values
self.turning_P = self.sd.getAutoUpdateValue("TurnToAngle/P", 1)
self.turning_I = self.sd.getAutoUpdateValue("TurnToAngle/I", 0)
self.turning_D = self.sd.getAutoUpdateValue("TurnToAngle/D", 0)
self.turn_controller = wpilib.PIDController(Kp=self.turning_P.value, Ki=self.turning_I.value, Kd=self.turning_D.value, source=self, output=self)
self.turn_controller.setInputRange(-180, 180)
self.turn_controller.setOutputRange(-1, 1)
self.turn_controller.setContinuous(True)
self.pid_value = 0
self.drive_constant = self.sd.getAutoUpdateValue("Drive/Drive Constant", 0.0001)
self.drive_multiplier = self.sd.getAutoUpdateValue("Drive/Drive Multiplier", 0.75)
SmartDashboard.putBoolean("Reversed", False)
self.reversed = False
self.logger = logging.getLogger("drive")
self.iErr = 0
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.Joystick(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.spin_rate = 0.3
def extra_processing(pipeline):
"""
Performs extra processing on the pipeline's outputs and publishes data to NetworkTables.
:param pipeline: the pipeline that just processed an image
:return: None
"""
center_x_positions = []
center_y_positions = []
widths = []
heights = []
# Find the bounding boxes of the contours to get x, y, width, and height
for contour in pipeline.filter_contours_output:
x, y, w, h = cv2.boundingRect(contour)
center_x_positions.append(
x + w / 2
) # X and Y are coordinates of the top-left corner of the bounding box
center_y_positions.append(y + h / 2)
widths.append(w)
heights.append(y)
print(center_x_positions)
# Publish to the '/vision' network table
table = NetworkTable.getTable("/vision")
table.putValue("centerX", NumberArray.from_list(center_x_positions))
table.putValue("centerY", NumberArray.from_list(center_y_positions))
table.putValue("width", NumberArray.from_list(widths))
table.putValue("height", NumberArray.from_list(heights))
def getTable(cls):
if cls.table is None:
from networktables import NetworkTable
cls.table = NetworkTable.getTable("SmartDashboard")
hal.report(hal.UsageReporting.kResourceType_SmartDashboard,
hal.UsageReporting.kSmartDashboard_Instance)
return cls.table
def put(self, key, value):
"""Puts the given value into the given key position
:param key: the key
:param value: the value
"""
if any((c in key) for c in "=\n\r\t[] "):
raise KeyError("improper preference key '%s'" % key)
with self.lock:
if key not in self.values:
self.keylist.append(key)
self.values[key] = value
try:
from networktables import NetworkTable
NetworkTable.getTable(self.TABLE_NAME).putString(key, value)
except ImportError:
pass
def __init__(self):
target = None
nt = NetworkTable.getTable('/camera')
nt.addTableListener(self._on_target, True, 'target')
self.aimed_at_angle = None
self.aimed_at_x = None
def put(self, key, value):
"""Puts the given value into the given key position
:param key: the key
:param value: the value
"""
if any((c in key) for c in "=\n\r\t[] "):
raise KeyError("improper preference key '%s'" % key)
with self.lock:
if key not in self.values:
self.keylist.append(key)
self.values[key] = value
try:
from networktables import NetworkTable
NetworkTable.getTable(self.TABLE_NAME).putString(key, value)
except ImportError:
pass
def __init__( self, capture, log, settings ):
# port="",filters="", hsv=False, original=True, in_file="", out_file="", display=True
self.limits = {}
# Pass the log object
self.log = log
log.init( "initializing saber_track Tracker" )
self.settings = settings
# If the port tag is True, set the
if settings["port"] != "":
logging.basicConfig( level=logging.DEBUG )
NetworkTable.setIPAddress( settings["port"] )
NetworkTable.setClientMode( )
NetworkTable.initialize( )
self.smt_dash = NetworkTable.getTable( "SmartDashboard" )
# initialize the filters. If the filter is the default: "", it will not create trackbars for it.
self.init_filters( settings["filters"] )
# Deal with inputs and outputs
self.settings["out_file"] = str( self.settings["out_file"] ) # set the file that will be written on saved
if settings["in_file"] != "":
self.log.init( "Reading trackfile: " + settings["in_file"] + ".json" )
fs = open( name + ".json", "r" ) # open the file under a .json extention
data = json.loads( fs.read() )
self.limits.update( data )
fs.close( )
# Localize the caputure object
self.capture = capture
# if there are any color limits (Upper and Lower hsv values to track) make the tracking code runs
self.track = len( self.limits ) > 0
self.log.info( "Tracking: " + str(self.track) )
def __init__(self):
print "Initializing NetworkTables..."
NetworkTable.setClientMode()
NetworkTable.setIPAddress("roborio-4761-frc.local")
NetworkTable.initialize()
self.table = NetworkTable.getTable("vision")
print "NetworkTables initialized!"
def initNetworktables():
logging.basicConfig(level=logging.DEBUG) # to see messages from networktables
NetworkTable.setIPAddress('127.0.0.1')
# NetworkTable.setIPAddress('roborio-5260.local')
NetworkTable.setClientMode()
NetworkTable.initialize()
return NetworkTable.getTable('Pi')
def setupNetworkTable(table, address): # To see messages from networktables, you must setup logging logging.basicConfig(level=logging.DEBUG) NetworkTable.setIPAddress(address) NetworkTable.setClientMode() NetworkTable.initialize() return NetworkTable.getTable(table)
def main():
cap = cv2.VideoCapture(1)
#Network Tables Setup
logging.basicConfig(level=logging.DEBUG)
NetworkTable.setIPAddress('10.32.56.2')
NetworkTable.setClientMode()
NetworkTable.initialize()
nt = NetworkTable.getTable('SmartDashboard')
while cap.isOpened():
_,frame=cap.read()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
#Range for green light reflected off of the tape. Need to tune.
lower_green = np.array(constants.LOWER_GREEN, dtype=np.uint8)
upper_green = np.array(constants.UPPER_GREEN, dtype=np.uint8)
#Threshold the HSV image to only get the green color.
mask = cv2.inRange(hsv, lower_green, upper_green)
#Gets contours of the thresholded image.
_,contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
#Draw the contours around detected object
cv2.drawContours(frame, contours, -1, (0,0,255), 3)
#Get centroid of tracked object.
#Check to see if contours were found.
if len(contours)>0:
#find largest contour
cnt = max(contours, key=cv2.contourArea)
#get center
center = get_center(cnt)
cv2.circle(frame, center, 3, (0,0,255), 2)
if(center[0] != 0 and center[1]!=0):
center_str_x = "x = "+str(center[0])
center_str_y = "y = "+str(center[1])
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(frame, "Center", constants.TEXT_COORDINATE_1, font, 0.7, (0,0,255), 2)
cv2.putText(frame, center_str_x, constants.TEXT_COORDINATE_2, font, 0.7, (0,0,255), 2)
cv2.putText(frame, center_str_y, constants.TEXT_COORDINATE_3, font, 0.7, (0,0,255), 2)
angle, direction = get_offset_angle(center[0], center[1])
cv2.putText(frame, "Angle: "+str(angle),constants.TEXT_COORDINATE_4, font, 0.7, (0,0,255), 2)
nt.putNumber('CameraAngle', angle)
cv2.putText(frame, "Turn "+direction, constants.TEXT_COORDINATE_5, font, 0.7, (0,0,255), 2)
if direction == right:
nt.putNumber('Direction', 0)
else:
nt.putNumber('Direction', 1)
#show image
cv2.imshow('frame',frame)
cv2.imshow('mask', mask)
cv2.imshow('HSV', hsv)
#close if delay in camera feed is too long
k = cv2.waitKey(1) & 0xFF
if k == 27:
break
cv2.destroyAllWindows()
def __init__(self):
self._speed = 0.0
self.current_deque = deque([0.0] * 3, 3) # Used to average currents over last n readings
self.log_queue = []
self.velocity_queue = []
self.previous_velocity = 0.0
self.shoot_time = None
self.sd = NetworkTable.getTable("SmartDashboard")
self.write_log = False
def __init__(self, address='localhost', flush_period=20e-3):
self.nt = NetworkTable.getTable('DriverStationControlBoard')
self.sw_vals_out = BooleanArray()
self.led_vals_in = BooleanArray()
self.ana_vals_out = NumberArray()
self.pwm_vals_in = NumberArray()
self.reset_table()
def __init__(self, log_dir):
self.match_running = False
self.time_start = None
self.time_stop = None
self.log_dir = log_dir
self.save_dir = None
self.sd = NetworkTable.getTable("SmartDashboard")
self.sd.addTableListener(self.value_changed)
def __init__(self):
self._speed = 0.0
self.current_deque = deque([0.0] * 3, 3) # Used to average currents over last n readings
self.log_queue = []
self.velocity_queue = []
self.previous_velocity = 0.0
self.shoot_time = None
self.sd = NetworkTable.getTable('SmartDashboard')
self.write_log = False
def initNetworktables(options):
if options.dashboard:
logger.info("Connecting to networktables in Dashboard mode")
NetworkTable.setDashboardMode()
else:
logger.info("Connecting to networktables at %s", options.robot)
NetworkTable.setIPAddress(options.robot)
NetworkTable.setClientMode()
NetworkTable.initialize()
logger.info("Networktables Initialized")
def setupVisionTable():
global visionTable
NetworkTable.setIPAddress("10.46.69.21")
NetworkTable.setClientMode()
NetworkTable.initialize()
visionTable = NetworkTable.getTable("vision")
setRunVision(False)
turnOffLight()
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = XboxController(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.drive_motor_a = CANTalon(2)
self.drive_motor_b = CANTalon(5)
self.drive_motor_c = CANTalon(4)
self.drive_motor_d = CANTalon(3)
self.gear_alignment_motor = CANTalon(14)
self.winch_motor = CANTalon(11)
self.winch_motor.setInverted(True)
self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=0,
reverseChannel=1)
# self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=3,
# reverseChannel=4)
self.gear_push_solenoid = wpilib.Solenoid(2)
self.gear_drop_solenoid = wpilib.Solenoid(3)
# self.gear_push_solenoid = wpilib.DoubleSolenoid(forwardChannel=1, reverseChannel=2)
# self.gear_drop_solenoid = wpilib.Solenoid(0)
self.test_trajectory = generate_trapezoidal_trajectory(
0, 0, 3, 0, Chassis.max_vel, 1, -1, Chassis.motion_profile_freq)
self.throttle = 1.0
self.direction = 1.0
self.led_dio = wpilib.DigitalOutput(1)
self.compressor = wpilib.Compressor()
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
self.xbox = xbox.XboxController(self.lstick)
self.rstick = wpilib.Joystick(1)
self.lmotor = wpilib.Jaguar(0)
self.rmotor = wpilib.Jaguar(1)
self.gyro = wpilib.AnalogGyro(1)
self.sd = NetworkTable.getTable('SmartDashboard')
def init_networktables(options):
if options.dashboard:
logger.info("Connecting to networktables in Dashboard mode")
NetworkTable.setDashboardMode()
else:
logger.info("Connecting to networktables at %s", options.robot)
NetworkTable.setIPAddress(options.robot)
NetworkTable.setClientMode()
NetworkTable.initialize()
logger.info("Networktables Initialized")
def __init__(self):
# TODO: StartCAPI()
# TODO: See if the next line is necessary
# Resource.RestartProgram()
try:
from networktables import NetworkTable
#NetworkTable.setServerMode() -- don't set this explicitly, it's the default.
except ImportError:
warnings.warn("networktables not found", ImportWarning)
NetworkTable = None
from .driverstation import DriverStation
self.ds = DriverStation.getInstance()
if NetworkTable is not None:
NetworkTable.getTable("") # forces network tables to initialize
NetworkTable.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", False)
self.__initialized = True
def __init__(self):
self._speed = 0.0
self.state = States.no_ball
self.current_deque = deque([0.0] * 3, 3) # Used to average currents over last n readings
self.log_queue = []
self.velocity_queue = []
self.intake_time = 0.0
self.previous_velocity = 0.0
self.shoot_time = None
self.sd = NetworkTable.getTable('SmartDashboard')
self.contact_time = time.time()
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)
