def main():
wsclients = set()
allKeys = dict()
def updateClients():
data = json.dumps(allKeys)
for ws in wsclients:
asyncio.run(ws.send(data))
def connectionListener(connected, info):
print(info, "; Connected=%s" % connected)
def valueChanged(key, value, isNew):
allKeys[key] = value
updateClients()
NetworkTables.addConnectionListener(connectionListener,
immediateNotify=True)
NetworkTables.addEntryListener(valueChanged)
async def wshandler(ws, path):
wsclients.add(ws)
await ws.wait_closed()
wsclients.remove(ws)
start_server = websockets.serve(wshandler, '127.0.0.1', 5678)
asyncio.get_event_loop().run_until_complete(start_server)
asyncio.get_event_loop().run_forever()
def run(self):
# Determine what IP to connect to
try:
server = sys.argv[1]
# Save the robot ip
if not os.path.exists(self.cache_file):
with open(self.cache_file, 'w') as fp:
fp.write(server)
except IndexError:
try:
with open(self.cache_file) as fp:
server = fp.read().strip()
except IOError:
print("Usage: logger.py [10.xx.yy.2]")
return
logger.info("NT server set to %s", server)
NetworkTables.initialize(server=server)
# Use listeners to receive the data
NetworkTables.addConnectionListener(self.connectionListener,
immediateNotify=True)
NetworkTables.addEntryListener(self.valueChanged)
# When new data is queued, write it to disk
while True:
name, data = self.queue.get()
with open(name, 'w') as fp:
json.dump(data, fp)
def main():
videostream = cv2.VideoCapture(0)
# we're a client of the robot
NetworkTables.initialize(server="localhost")
self.robotStateTable = NetworkTables.getTable("/SmartDashboard/RobotState")
self.robotStateTable = NetworkTables.addEntryListener(updateRobotState)
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
# Display the resulting frame
cv2.imshow('frame', frame)
# here's where process the frame
#
# (success, rotVec, xlatVec) = cv2.solvePnP(...)
#
rotVec = np.array([1,2,])
(rotmat, _) = cv2.Rodrigues(rotVec)
# after processing the frame we convert our result into
# a form that the robot code can use
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def connect():
if STATE.connect_handle:
STATE.mainGUI.after_cancel(STATE.connect_handle)
if STATE.team_number.get() != 0:
NetworkTables.startClientTeam(STATE.team_number.get())
else:
NetworkTables.initialize(server="localhost")
NetworkTables.addConnectionListener(RUNNER.connectionListener,
immediateNotify=True)
NetworkTables.addEntryListener(RUNNER.valueChanged)
STATE.connected.set("Connecting...")
waitForConnection()
def run(self):
# Determine what IP to connect to
try:
server = sys.argv[1]
# Save the robot ip
if not os.path.exists(self.cache_file):
with open(self.cache_file, 'w') as fp:
fp.write(server)
except IndexError:
try:
with open(self.cache_file) as fp:
server = fp.read().strip()
except IOError:
print("Usage: logger.py [10.xx.yy.2]")
return
logger.info("NT server set to %s", server)
NetworkTables.initialize(server=server)
# Use listeners to receive the data
NetworkTables.addConnectionListener(self.connectionListener,
immediateNotify=True)
NetworkTables.addEntryListener(self.valueChanged)
# When new data is queued, write it to disk
while True:
name, data = self.queue.get()
with open(name, 'w') as fp:
fp.write(
"timestamp,robot_x,robot_y,robot_yaw,path_x,path_y,path_yaw,path_x_vel,path_y_vel,path_x_accel,"
"path_y_accel,robot_x_vel,robot_y_vel,x_error,y_error,yaw_error,x_power,y_power,yaw_power\n"
)
for line in data:
fp.write(",".join([str(f) for f in list(line)]) + "\n")
name, data = self.queue_pref.get()
with open(name, 'w') as fp:
fp.write(json.dumps(data))
# values, which prints out all changes. Note that the keys are full paths, and
# not just individual key values.
#
import sys
import time
from networktables import NetworkTables
# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)
if len(sys.argv) != 2:
print("Error: specify an IP to connect to!")
exit(0)
ip = sys.argv[1]
NetworkTables.initialize(server=ip)
def valueChanged(key, value, isNew):
print("valueChanged: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
NetworkTables.addEntryListener(valueChanged)
while True:
time.sleep(1)
def robotInit(self):
self.BUTTON_RBUMPER = 6
self.BUTTON_LBUMPER = 5
self.BUTTON_A = 1
self.BUTTON_B = 2
self.BUTTON_X = 3
self.BUTTON_Y = 4
self.LY_AXIS = 1
self.LX_AXIS = 0
self.RX_AXIS = 4
self.RY_AXIS = 5
self.R_TRIGGER = 3
self.L_TRIGGER = 2
self.LEFT_JOYSTICK_BUTTON = 9
self.RIGHT_JOYSTICK_BUTTON = 10
self.BACK_BUTTON = 7
self.START_BUTTON = 8
self.rev_per_ft = 12 / (math.pi * self.wheel_diameter)
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.arm = ctre.wpi_talonsrx.WPI_TalonSRX(0)
self.front_lift = ctre.wpi_talonsrx.WPI_TalonSRX(6)
self.front_lift_slave = ctre.wpi_talonsrx.WPI_TalonSRX(50)
self.front_lift_slave.follow(self.front_lift)
self.back_lift = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.back_lift_wheel = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.fl_motor.setInverted(True)
self.bl_motor.setInverted(True)
self.arm.setInverted(True)
self.fl_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.bl_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.fr_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.br_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.front_lift.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.back_lift.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
MyRobot.TIMEOUT_MS)
# Reverse negative encoder values
self.fl_motor.setSensorPhase(True)
#self.fr_motor.setSensorPhase(True)
self.br_motor.setSensorPhase(True)
self.front_lift.setSensorPhase(True)
self.deadzone_amount = 0.15
self.control_state = "speed"
self.start_navx = 0
self.previous_hyp = 0
self.js_startAngle = 0
self.rot_startNavx = 0
self.joystick = wpilib.Joystick(0)
NetworkTables.addEntryListener(self.entry_listener)
self.use_pid = False
self.prev_pid_toggle_btn_value = False
self.navx = navx.AHRS.create_spi()
self.relativeGyro = RelativeGyro(self.navx)
self.timer = wpilib.Timer()
self.arm_pot = wpilib.AnalogPotentiometer(0)
self.arm_pid = wpilib.PIDController(3, 0, 0, self.arm_pot.get,
self.pid_output)
self.init_time = 0
self.desired_rate = 0
self.pid_turn_rate = 0
self.prev_button1 = False
self.button = False
self.button_chomp = False
self.front_lift_heights_index = 0
self.lift_target = 0
self.driveStates = {
'velocity': Velocty_Mode(self),
'enter_position': Enter_Position_Mode(self),
'position': Position_Mode(self),
'enter_rotation': Enter_Rotation_Mode(self),
'rotation': Rotation_Mode(self),
'leave_special': Leave_Special_Mode(self),
'lift_robot': Lift_Robot(self)
}
self.drive_sm = State_Machine(self.driveStates, "Drive_sm")
self.drive_sm.set_state('velocity')
self.liftStates = {
'fully_raised': Fully_Raised(self),
'middle': Middle(self),
'fully_lowered': Fully_Lowered(self),
'go_to_height': Go_To_Height(self)
}
self.lift_sm = State_Machine(self.liftStates, "lift_sm")
self.lift_sm.set_state('fully_lowered')
self.wheel_motors = [
self.br_motor, self.bl_motor, self.fr_motor, self.fl_motor
]
wpilib.CameraServer.launch()
def normalized_navx():
return self.get_normalized_angle(self.navx.getAngle())
self.angle_pid = wpilib.PIDController(self.turn_rate_p,
self.turn_rate_i,
self.turn_rate_d,
self.relativeGyro.getAngle,
self.set_pid_turn_rate)
#self.turn_rate_pid.
#self.turn_rate_pid.
self.angle_pid.setInputRange(-self.turn_rate_pid_input_range,
self.turn_rate_pid_input_range)
self.angle_pid.setOutputRange(-self.turn_rate_pid_output_range,
self.turn_rate_pid_output_range)
self.angle_pid.setContinuous(True)
self.turn_rate_values = [0] * 10
def run(self):
# Initialize networktables
team = ""
while team == "":
team = input("Enter team number or 'sim': ")
if team == "sim":
NetworkTables.initialize(server="localhost")
else:
NetworkTables.startClientTeam(int(team))
# Use listeners to receive the data
NetworkTables.addConnectionListener(self.connectionListener,
immediateNotify=True)
NetworkTables.addEntryListener(self.valueChanged)
# Wait for a connection notification, then continue on the path
print("Waiting for NT connection..")
while True:
if self.queue.get() == "connected":
break
print("Connected!")
print()
autonomous = [
# name, initial speed, ramp
("slow-forward", 0, 0.001),
("slow-backward", 0, -0.001),
("fast-forward", abs(ROBOT_FAST_SPEED), 0),
("fast-backward", -abs(ROBOT_FAST_SPEED), 0),
]
stored_data = {}
#
# Wait for the user to cycle through the 4 autonomus modes
#
for i, (name, initial_speed, ramp) in enumerate(autonomous):
# Initialize the robot commanded speed to 0
self.autospeed = 0
self.discard_data = True
print()
print("Autonomous %d/%d: %s" % (i + 1, len(autonomous), name))
print()
print("Please enable the robot in autonomous mode.")
print()
print(
"WARNING: It will not automatically stop moving, so disable the robot"
)
print("before it hits something!")
print("")
# Wait for robot to signal that it entered autonomous mode
with self.lock:
self.lock.wait_for(lambda: self.mode == "auto")
data = self.wait_for_stationary()
if data is not None:
if data in ("connected", "disconnected"):
print(
"ERROR: NT disconnected, results won't be reliable. Giving up."
)
return
else:
print(
"Robot exited autonomous mode before data could be sent?"
)
break
# Ramp the voltage at the specified rate
data = self.ramp_voltage_in_auto(initial_speed, ramp)
if data in ("connected", "disconnected"):
print(
"ERROR: NT disconnected, results won't be reliable. Giving up."
)
return
# output sanity check
if len(data) < 3:
print(
"WARNING: There wasn't a lot of data received during that last run"
)
else:
left_distance = data[-1][L_ENCODER_P_COL] - data[0][
L_ENCODER_P_COL]
right_distance = data[-1][R_ENCODER_P_COL] - data[0][
R_ENCODER_P_COL]
print()
print("The robot reported traveling the following distance:")
print()
print("Left: %.3f ft" % left_distance)
print("Right: %.3f ft" % right_distance)
print()
print(
"If that doesn't seem quite right... you should change the encoder calibration"
)
print("in the robot program or fix your encoders!")
stored_data[name] = data
# In case the user decides to re-enable autonomous again..
self.autospeed = 0
#
# We have data! Do something with it now
#
# Write it to disk first, in case the processing fails for some reason
# -> Using JSON for simplicity, maybe add csv at a later date
now = time.strftime("%Y%m%d-%H%M-%S")
fname = "%s-data.json" % now
print()
print("Data collection complete! saving to %s..." % fname)
with open(fname, "w") as fp:
json.dump(stored_data, fp, indent=4, separators=(",", ": "))
analyze_data(stored_data)
conn_status = b"CONNECTED"
conn_color = pynk.lib.nk_rgb(0, 255, 0)
def drawValues(ctx, inValues):
for k in inValues.keys():
pynk.lib.nk_layout_row_dynamic(ctx, 0, 2)
pynk.lib.nk_label(ctx, k.encode('utf-8'), pynk.lib.NK_TEXT_LEFT)
pynk.lib.nk_label(ctx, str(inValues[k]).encode('utf-8'), pynk.lib.NK_TEXT_RIGHT)
values = {}
def entryListener(key, value, isNew): # isNew is if new entry
values[key] = value
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
NetworkTables.addEntryListener(entryListener)
if __name__ == '__main__':
# Initialise pygame.
pygame.init()
screen = make_screen()
pygame.display.set_caption(WIN_CAPTION)
running = True
# Initialise nuklear
font = pynk.nkpygame.NkPygameFont(
pygame.font.SysFont(FONT_NAME, FONT_SIZE))
with pynk.nkpygame.NkPygame(font) as nkpy:
text_color = pynk.lib.nk_rgb(
nkpy.ctx.style.text.color.r, nkpy.ctx.style.text.color.g, nkpy.ctx.style.text.color.b)
def EventListener(self,listener):
NT.addEntryListener(listener)
def robotInit(self):
self.BUTTON_RBUMPER = 6
self.BUTTON_LBUMPER = 5
self.LY_AXIS = 1
self.LX_AXIS = 0
self.RX_AXIS = 4
self.RY_AXIS = 5
self.rev_per_ft = 12 / (math.pi * self.wheel_diameter)
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(40)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(50)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(30)
self.br_motor.setInverted(True)
self.fr_motor.setInverted(True)
self.fl_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.bl_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.fr_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.br_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.fr_motor.setSensorPhase(False) # Reverse negative encoder values
self.br_motor.setSensorPhase(True)
self.deadzone_amount = 0.15
self.control_state = "speed"
self.start_navx = 0
self.previous_hyp = 0
self.js_startAngle = 0
self.rot_startNavx = 0
self.spinman = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.littlearms1 = wpilib.Servo(7)
self.littlearms2 = wpilib.Servo(8)
self.joystick = wpilib.Joystick(0)
NetworkTables.addEntryListener(self.entry_listener)
self.use_pid = False
self.prev_pid_toggle_btn_value = False
self.navx = navx.AHRS.create_spi()
self.relativeGyro = RelativeGyro(self.navx)
self.timer = wpilib.Timer()
self.init_time = 0
self.desired_rate = 0
self.pid_turn_rate = 0
def normalized_navx():
return self.get_normalized_angle(self.navx.getAngle())
self.angle_pid = wpilib.PIDController(self.turn_rate_p, self.turn_rate_i, self.turn_rate_d, self.relativeGyro.getAngle, self.set_pid_turn_rate)
#self.turn_rate_pid.
#self.turn_rate_pid.
self.angle_pid.setInputRange(-self.turn_rate_pid_input_range, self.turn_rate_pid_input_range)
self.angle_pid.setOutputRange(-self.turn_rate_pid_output_range, self.turn_rate_pid_output_range)
self.angle_pid.setContinuous(True)
self.turn_rate_values = [0] * 10
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.rev_per_ft = 12 / (math.pi * self.wheel_diameter)
self.ticks_per_ft_left = self.rev_per_ft * self.ticks_per_rev_left
self.ticks_per_ft_right = self.rev_per_ft * self.ticks_per_rev_right
self.TIMEOUT_MS = 30
self.BUTTON_A = 1
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.fr_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, self.TIMEOUT_MS)
self.fl_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, self.TIMEOUT_MS)
# maybe this can be used to scale encoder values to ft?
#self.fl_motor.configSelectedFeedbackCoefficient(2, 0, 0)
self.fl_motor.setSensorPhase(True)
self.fr_motor.setSensorPhase(True)
self.br_motor.follow(self.fr_motor)
self.bl_motor.follow(self.fl_motor)
self.fr_motor.setInverted(True)
self.br_motor.setInverted(True)
self.fl_motor.setSensorPhase(True)
self.fr_motor.setSensorPhase(True)
#initial positions for position modes
self.initial_position_left = 0
self.initial_position_right = 0
#self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.bl_motor, self.fr_motor, self.br_motor)
self.joystick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
self.last_seen = 0
wpilib.CameraServer.launch('vision.py:main')
limelight_table = NetworkTables.getTable("limelight")
limelight_table.putNumber('ledMode', 1)
self.pid = wpilib.PIDController(self.P, self.I, self.D, self.pid_source, self.pid_output)
self.pid.setOutputRange(-1, 1)
self.pid.setInputRange(-15, 45)
self.turn_rate = 0
self.position_mode_toggle = False
NetworkTables.addEntryListener(self.entry_listener)
def getKeys(table, types=0):
return table.getKeys(types)
def getEntry(table, key):
return table.getEntry(key)
def getValue(entry):
return entry.value
def getValueString(entry):
return repr(entry.value)
def getType(entry):
return types[entry.getType()]
handler = None
# Note: key is full path
def callback(key, value, isNew): # (str, Any, bool)
return handler.sendEmptyMessage(0)
NT.addEntryListener(callback)
