search

robotpy / pynetworktables

Pure python implementation of the FRC NetworkTables protocol
Other
60 stars 30 forks source link

Is there a way to start the robot in Teleoperated, Autonomous, Practice, or Test mode? #121

Closed Mockapapella closed 2 years ago

Mockapapella commented 2 years ago

I'm trying to start my robot in test mode using pynetworktables on Linux (Ubuntu 20.04) for the purposes of building my own custom dashboard. I've initialized a network table server on my RoboRIO with a basic counter that updates every cycle:

import wpilib
import ctre
from networktables import NetworkTables
import logging

logging.basicConfig(level=logging.DEBUG)

class MyRobot(wpilib.TimedRobot):
    # Setup networktables
    NetworkTables.initialize()
    smartdash = NetworkTables.getTable("SmartDashboard")
    count = 0
    smartdash.putNumber("Count", count)

    #: update every 0.005 seconds/5 milliseconds (200Hz)
    kUpdatePeriod = 0.005

    def testInit(self):
        """Robot initialization function"""

        self.motor = ctre.WPI_TalonSRX(0)  # initialize the motor as a Talon on channel 0
        self.joyStick = wpilib.Joystick(0)  # initialize the joystick on port 0

    def testPeriodic(self):
        """Runs the motor from a joystick."""

        # Set the motor's output.
        self.motor.set(((self.joyStick.getThrottle() + 1) / 2) * 0.25)
        self.smartdash.putNumber("Count", self.count)
        self.count += 1

if __name__ == "__main__":
    wpilib.run(MyRobot)

And then I have a client.py file that reads the Count table:

import sys
import time
from networktables import NetworkTables
import threading
import logging

logging.basicConfig(filename="logs.txt", filemode="a", level=logging.DEBUG)

if len(sys.argv) != 2:
    print("Error: specify an IP to connect to!")
    exit(0)

ip = sys.argv[1]

cond = threading.Condition()
notified = [False]

def connectionListener(connected, info):
    print(info, "; Connected=%s" % connected)
    with cond:
        notified[0] = True
        cond.notify()

NetworkTables.initialize(server=ip)
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)

with cond:
    print("Waiting")
    if not notified[0]:
        cond.wait()

def valueChanged(key, value, isNew):
    logging.debug(f"valueChanged: key: {key}; value: {value}; isNew: {isNew}")

NetworkTables.addEntryListener(valueChanged)

while True:
    try:
        count = smartdash.getNumber("Count", -1)
        print(count)
        time.sleep(1)
    except KeyboardInterrupt as e:
        exit()

As expected, it prints out the current Count value from the network table when I start the robot in test mode with the third party driver station QDriverStation. Button presses and other logs with QDriverStation show up in my logs.txt file because it's affecting the network tables. I want to emulate this behavior using pynetworktables in my client.py file. Looking at the different network tables that are changed when I start the robot in test mode with QDriverStation I see this:

<START CLIENT.PY>
DEBUG:nt:client trying to connect
DEBUG:nt:client connected
DEBUG:nt:client: sending hello
DEBUG:nt:notifying '/FMSInfo/.type' (local=0), flags=4
DEBUG:nt:notifying '/FMSInfo/IsRedAlliance' (local=1), flags=4
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=4
DEBUG:nt:notifying '/FMSInfo/EventName' (local=3), flags=4
DEBUG:nt:notifying '/FMSInfo/GameSpecificMessage' (local=4), flags=4
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=4
DEBUG:nt:notifying '/FMSInfo/MatchNumber' (local=6), flags=4
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=7), flags=4
DEBUG:nt:notifying '/FMSInfo/ReplayNumber' (local=8), flags=4
DEBUG:nt:notifying '/FMSInfo/StationNumber' (local=9), flags=4
DEBUG:nt:notifying '/FMSInfo/MatchType' (local=10), flags=4
INFO:nt:client: CONNECTED to server 192.168.1.81 port 1735
DEBUG:nt:notifying '/FMSInfo/.type' (local=0), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/.type; value: FMSInfo; isNew: 1
DEBUG:nt:notifying '/FMSInfo/IsRedAlliance' (local=1), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/IsRedAlliance; value: True; isNew: 1
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=5
DEBUG:nt:notifying '/FMSInfo/EventName' (local=3), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 0.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/GameSpecificMessage' (local=4), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/EventName; value: ; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/GameSpecificMessage; value: ; isNew: 1
DEBUG:nt:notifying '/FMSInfo/MatchNumber' (local=6), flags=5
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 0.0; isNew: 1
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=7), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/MatchNumber; value: 0.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/ReplayNumber' (local=8), flags=5
DEBUG:nt:notifying '/FMSInfo/StationNumber' (local=9), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/.status/LW Enabled; value: False; isNew: 1
DEBUG:nt:notifying '/FMSInfo/MatchType' (local=10), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/ReplayNumber; value: 0.0; isNew: 1
DEBUG:root:valueChanged: key: /FMSInfo/StationNumber; value: 1.0; isNew: 1
DEBUG:root:valueChanged: key: /FMSInfo/MatchType; value: 0.0; isNew: 1
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
...

<OPEN QDriverStation, TELEOPERATED AUTOMATICALLY SELECTED AND DISABLED>
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 32.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=16
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
...

<SELECT Test, AUTOMATICALLY DISABLED>
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=16
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 36.0; isNew: 1
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
...

<ENABLED ROBOT>
DEBUG:root:valueChanged: key: /LiveWindow/.status/LW Enabled; value: True; isNew: 1
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=7), flags=16
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 37.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=16
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.name; value: Talon SRX [0]; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.name' (local=11), flags=4
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.type; value: Speed Controller; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.type' (local=12), flags=4
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/.type; value: LW Subsystem; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/.type' (local=13), flags=4
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/Value; value: 0.125; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/Value' (local=14), flags=4
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 5.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 10.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 15.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 20.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 25.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 30.0; isNew: 1
DEBUG:nt:dispatch running 1 connections
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 35.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
...

<DISABLE ROBOT>
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 36.0; isNew: 1
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.controllable; value: False; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=5), flags=16
DEBUG:root:valueChanged: key: /LiveWindow/.status/LW Enabled; value: False; isNew: 1
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=2), flags=16
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.controllable' (local=15), flags=4
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=7), flags=16
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections
<STOP CLIENT>

Using that information, I'm trying to start the robot in test mode upon client connection. Modified client.py:

...
with cond:
    print("Waiting")
    if not notified[0]:
        cond.wait()

smartdash = NetworkTables.getTable("/SmartDashboard")

# FMSInfo Variables
fms_stuff = NetworkTables.getTable("/FMSInfo")
fms_stuff.putNumber("FMSControlData", 37)
fms_stuff.putNumber("MatchType", 1)

# LiveWindow Table Variables
live_window = NetworkTables.getTable("/LiveWindow")
live_window_subtable_status = live_window.getSubTable(".status")
live_window_subtable_ungrouped = live_window.getSubTable("Ungrouped")
live_window_subtable_ungrouped_talon_name = live_window_subtable_ungrouped.getSubTable(
    "Talon SRX [0]"
)
live_window_subtable_ungrouped_talon_type = live_window_subtable_ungrouped.getSubTable(
    "Talon SRX [0]"
)
# live_window_subtable_ungrouped_type = live_window_subtable_ungrouped.getSubTable(".type")
# live_window_subtable_ungrouped_name = live_window_subtable_ungrouped.getSubTable("Value")

live_window_subtable_status.putBoolean("LW Enabled", True)
live_window_subtable_ungrouped_talon_name.putString(".name", "Talon SRX [0]")
live_window_subtable_ungrouped_talon_type.putString(".type", "Speed Controller")
live_window_subtable_ungrouped.putString(".type", "LW Subsystem")
live_window_subtable_ungrouped.putNumber("Value", 0.125)

def valueChanged(key, value, isNew):
    logging.debug(f"valueChanged: key: {key}; value: {value}; isNew: {isNew}")
...

Basically I'm trying to mimic what QDriverStation is doing except with Python. When I run client.py with the modifications above, this is what shows up in logs.txt:

DEBUG:nt:client trying to connect
DEBUG:nt:client connected
DEBUG:nt:client: sending hello
DEBUG:nt:notifying '/FMSInfo/.type' (local=0), flags=4
DEBUG:nt:notifying '/FMSInfo/IsRedAlliance' (local=1), flags=4
DEBUG:nt:notifying '/FMSInfo/MatchNumber' (local=2), flags=4
DEBUG:nt:notifying '/FMSInfo/ReplayNumber' (local=3), flags=4
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.controllable' (local=4), flags=4
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.name' (local=5), flags=4
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=6), flags=4
DEBUG:nt:notifying '/FMSInfo/EventName' (local=7), flags=4
DEBUG:nt:notifying '/FMSInfo/GameSpecificMessage' (local=8), flags=4
DEBUG:nt:notifying '/SmartDashboard/Count' (local=9), flags=4
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=10), flags=4
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.type' (local=11), flags=4
DEBUG:nt:notifying '/FMSInfo/StationNumber' (local=12), flags=4
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/Value' (local=13), flags=4
DEBUG:nt:notifying '/LiveWindow/Ungrouped/.type' (local=14), flags=4
DEBUG:root:valueChanged: key: /FMSInfo/.type; value: FMSInfo; isNew: 1
DEBUG:nt:notifying '/FMSInfo/MatchType' (local=15), flags=4
DEBUG:root:valueChanged: key: /FMSInfo/IsRedAlliance; value: True; isNew: 1
INFO:nt:client: CONNECTED to server 192.168.1.81 port 1735
DEBUG:root:valueChanged: key: /FMSInfo/MatchNumber; value: 0.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/.type' (local=0), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/ReplayNumber; value: 0.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/IsRedAlliance' (local=1), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Value; value: 0.125; isNew: 1
DEBUG:nt:notifying '/FMSInfo/MatchNumber' (local=2), flags=5
DEBUG:nt:notifying '/FMSInfo/ReplayNumber' (local=3), flags=5
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Value' (local=16), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.name; value: Talon SRX [0]; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.name' (local=5), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.type; value: Speed Controller; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.type' (local=11), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/FMSControlData; value: 37.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/FMSControlData' (local=6), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/.controllable; value: False; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/.controllable' (local=4), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/EventName; value: ; isNew: 1
DEBUG:nt:notifying '/FMSInfo/EventName' (local=7), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/GameSpecificMessage; value: ; isNew: 1
DEBUG:nt:notifying '/FMSInfo/GameSpecificMessage' (local=8), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/.status/LW Enabled; value: True; isNew: 1
DEBUG:nt:notifying '/LiveWindow/.status/LW Enabled' (local=10), flags=5
DEBUG:root:valueChanged: key: /SmartDashboard/Count; value: 339.0; isNew: 1
DEBUG:nt:notifying '/SmartDashboard/Count' (local=9), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/StationNumber; value: 1.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/StationNumber' (local=12), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Talon SRX [0]/Value; value: 0.125; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Talon SRX [0]/Value' (local=13), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/.type; value: LW Subsystem; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/.type' (local=14), flags=5
DEBUG:root:valueChanged: key: /FMSInfo/MatchType; value: 1.0; isNew: 1
DEBUG:nt:notifying '/FMSInfo/MatchType' (local=15), flags=5
DEBUG:root:valueChanged: key: /LiveWindow/Ungrouped/Value; value: 0.125; isNew: 1
DEBUG:nt:notifying '/LiveWindow/Ungrouped/Value' (local=16), flags=16
DEBUG:nt:dispatch running 1 connections
DEBUG:nt:dispatch running 1 connections

At this point I'm not sure if it's just something small that I've overlooked like a single table value or if it's something else entirely, but I feel like I'm on the right path. Any help would be much appreciated.

virtuald commented 2 years ago

If I understand your question correctly, you cannot control the robot's mode via NetworkTables. QDriverStation controls the mode of the robot using the DS protocol, not through NetworkTables. You would need to write your own DS code to do this.

Mockapapella commented 2 years ago

Well that kind of sucks. Just to clarify a few things:

  1. The driver station protocol isn't open source, right? So that implementation (and some others I've seen) are all reverse engineered?
  2. If I were to participate in a match with my robot, I would be required per FIRST rules to use the official drivers station, but I could use my own custom dashboard displaying networktable data (and other things like camera feeds), right?
TheTripleV commented 2 years ago

Yes, and yes.

Mockapapella commented 2 years ago

Alright, thank you for the extra context and the help.