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 _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, ip, name):
NetworkTable.setIPAddress(ip)
NetworkTable.setClientMode()
NetworkTable.initialize()
self.visionNetworkTable = NetworkTable.getTable(name)
if constants.SENDTOSMARTDASHBOARD:
constants.smartDashboard = NetworkTable.getTable("SmartDashboard")
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)
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 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):
"""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 __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 __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, 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, 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 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 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 __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()
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 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 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 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 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 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 __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, 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 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, 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, 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 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 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 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 __init__(self):
print "Initializing NetworkTables..."
NetworkTable.setClientMode()
NetworkTable.setIPAddress("roborio-4761-frc.local")
NetworkTable.initialize()
self.table = NetworkTable.getTable("vision")
print "NetworkTables initialized!"
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 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 setupVisionTable():
global visionTable
NetworkTable.setIPAddress("10.46.69.21")
NetworkTable.setClientMode()
NetworkTable.initialize()
visionTable = NetworkTable.getTable("vision")
setRunVision(False)
turnOffLight()
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 main():
ip = "10.24.81.2"
NetworkTable.setIPAddress(ip)
NetworkTable.setClientMode()
#print "Initializing Network Tables"
NetworkTable.initialize()
goalFinder = GoalFinder()
stream = urllib.urlopen('http://10.24.81.11/mjpg/video.mjpg')
bytes = ''
#print "Start Target Search Loop..."
#turn true for single picture debuging
first = False
beagle = NetworkTable.getTable("GRIP")
goals_table = beagle.getSubTable("aGoalContours")
while True:
#TODO: Fetch image from camera.
# img = cv2.imread("0.jpg")
bytes += stream.read(16384)
b = bytes.rfind('\xff\xd9')
a = bytes.rfind('\xff\xd8', 0, b - 1)
if a != -1 and b != -1:
jpg = bytes[a:b + 2]
bytes = bytes[b + 2:]
img = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8),
cv2.CV_LOAD_IMAGE_COLOR)
goalFinder.process_image(img)
goals_table.putValue("centerX",
NumberArray.from_list(goalFinder.targetXs))
goals_table.putValue("centerY",
NumberArray.from_list(goalFinder.targetYs))
goals_table.putValue(
"width", NumberArray.from_list(goalFinder.targetWidths))
goals_table.putValue(
"height", NumberArray.from_list(goalFinder.targetHeights))
goals_table.putValue("area",
NumberArray.from_list(goalFinder.targetAreas))
#if len(goalFinder.targetAreas) > 0:
# print goalFinder.targetAreas
#Use if you want to the save the image and retrieve it later.
if first:
first = False
cv2.imwrite("test.jpg", img)
goals_table.putNumber("OwlCounter",
goals_table.getNumber("OwlCounter", 0) + 1)
sleep(.01)
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):
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 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_ntable():
global ntable
NetworkTable.setIPAddress(config.server["rioAddress"])
NetworkTable.setClientMode()
NetworkTable.initialize()
ntable = NetworkTable.getTable('vision');
ntable.addTableListener(onValueChanged)
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_networktables():
#TODO: Check to see if NetworkTables is already initialized
if not using_networktables:
raise Exception("Attempted to initialize NetworkTables while NetworkTables is disabled!")
NetworkTable.setIPAddress(args.networktables_ip)
NetworkTable.setClientMode()
NetworkTable.initialize()
global table
table = NetworkTable.getTable("vision")
log.info("Initialized NetworkTables")
def drive_distance(self, initial_call):
if initial_call:
self.drive.field_centric = False
self.sd = NetworkTable.getTable('SmartDashboard')
self.drive.enable_position_prediction()
self.x_ctrl.move_to(self.drive_distance_feet)
if self.x_ctrl.is_at_location():
self.next_state('finish')
def __init__(self,
drive_motor,
rotate_motor,
encoder,
sd_prefix='SwerveModule',
inverted=False,
zero=0.0,
allow_reverse=True,
has_drive_encoder=False):
"""
Swerve drive module was written for a swerve drive train that uses absolute encoders for tracking wheel rotation.
:param drive_motor: Motor object
:param rotate_motor: Motor object
:param encoder: AnalogInput wpilib object
:param sd_prefix: a string used to differentiate modules when debugging
:param zero: The default zero for the encoder
:param inverted: boolean to invert the wheel rotation
:param allow_reverse: If true allows wheels to spin backwards instead of rotating
:param has_drive_encoder: If true allows the module to track wheel position
"""
# SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
self.sd_prefix = sd_prefix
# Motors
self.drive_motor = drive_motor
self.drive_inverted = inverted
self.drive_motor.setInverted(self.drive_inverted)
self.rotate_motor = rotate_motor
self._requested_voltage = 0 # Angle in voltage form
self._requested_speed = 0
# Encoder
self.encoder = encoder
self.encoder_zero = zero
# PID
self._pid_controller = wpilib.PIDController(1.5, 0.0, 0.0,
self.encoder,
self.rotate_motor)
self._pid_controller.setContinuous()
self._pid_controller.setInputRange(0.0, 5.0)
self._pid_controller.enable()
# State variables
self.allow_reverse = allow_reverse
self.debugging = self.sd.getAutoUpdateValue(
'drive/%s/debugging' % self.sd_prefix, False)
self.has_drive_encoder = has_drive_encoder
def __init__(self):
# mjpg-streamer isn't setting parameters properly yet, so do it here
if not hal.HALIsSimulation(): # pragma: no cover
from vision.vision import setCaptureParameters
setCaptureParameters("/dev/v4l/by-id/usb-046d_0825_96EBCE50-video-index0",
"/etc/default/mjpg-streamer")
self.nt = NetworkTable.getTable('vision')
self._values = {'x': 0.0, 'y': 0.0, 'w': 0.0, 'h': 0.0, 'time': 0.0}
self._smoothed_pidget = 0.0
self.no_vision_counter = 0
self.nt.addTableListener(self.valueChanged)
def setup(self):
"""
Called after injection.
"""
#Put all the modules into a dictionary
self.modules = {
'front_right': self.fr_module,
'front_left': self.fl_module,
'rear_left': self.rl_module,
'rear_right': self.rr_module
}
self.sd = NetworkTable.getTable('SmartDashboard')
self._requested_vectors = {
'fwd': 0,
'strafe': 0,
'rcw': 0
}
self._requested_angles = {
'front_right': 0,
'front_left': 0,
'rear_left': 0,
'rear_right': 0
}
self._requested_speeds = {
'front_right': 0,
'front_left': 0,
'rear_left': 0,
'rear_right': 0
}
self._predicted_position = {
'fwd': 0,
'strafe': 0,
'rcw': 0
}
self.predict_position = False
# Variables that allow enabling and disabling of features in code
self.allow_reverse = False
self.squared_inputs = True
self.snap_rotation = False
self.wait_for_align = False
self.threshold_input_vectors = True
self.width = (22/12)/2
self.length = (18.5/12)/2
self.request_wheel_lock = False
def drive_distance(self, initial_call):
if initial_call:
self.drive.field_centric = False
self.sd = NetworkTable.getTable('SmartDashboard')
self.tracker.enable()
self.y_ctrl.move_to(self.drive_distance_feet)
self.sd.putNumber('TESTING: ', self.y_ctrl.get_position())
if self.y_ctrl.is_at_location():
self.next_state('finish')
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.sd = NetworkTable.getTable('/SmartDashboard')
self.angle_P = self.sd.getAutoUpdateValue('Drive/Angle_P', .055)
self.angle_I = self.sd.getAutoUpdateValue('Drive/Angle_I', 0)
self.drive_constant = self.sd.getAutoUpdateValue('Drive/Drive_Constant', .0001)
self.rotate_max = self.sd.getAutoUpdateValue('Drive/Max Gyro Rotate Speed', .37)
self.enabled = False
self.align_angle = None
self.align_print_timer = wpilib.Timer()
self.align_print_timer.start()
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)
def setup(robot_ip, table_id, connection_listener_class=None):
print("Connecting to NetworkTable '{}' on Robot at '{}'".format(table_id, robot_ip))
NetworkTable.setIPAddress(robot_ip)
NetworkTable.setClientMode()
NetworkTable.initialize()
table = NetworkTable.getTable(table_id)
if connection_listener_class:
table.addConnectionListener(connection_listener_class())
return table
