The Project:
Jusnoor Dhillon / (851 Words, 5 Minutes)
The Project:
The goal of this project was to program a holonomic tandem driving solution that would allow a user to control two NERDSwerve robots from a single joystick. Another intern and I built this system using OrangePis and AprilTags, through the FRC ecosystem. It took place in the summer of 2025.
Technical writeup:
Networking
Both RoboRIOs, both OrangePis, and the Driver Station operate in a single VLAN, 10.93.12.1/24. The FIRST control scheme prohibits the driver station to connect to multiple rios, so all user input is first sent to the master, and then dispersed to the slave. Inter-robot communication is done through NetworkTables (NT), which is primarily used by teams for telemetry; by default, each rio hosts a NT server, and telemetry viewing applications on the Driver Station host NT clients. In this scheme, the master robot hosts the server; and the master, slave, and Driver Station applications host clients. The robots are currently set up to have VividHosting radios, which are fairly locked-down (they are not meant for controlling more than one robot). Currently, this is the limiting factor to adding more robots to the system. However, these radios can easily be replaced by routers running a simple mesh network (minimal setup).
Having the master robot host a client allows for code interoperability between the master and slave, as well as getting rid of some master/slave latency. The values transmitted between robots over NT include enabled signal, controller outputs, master position (for pose estimation), the global target position (for tandem driving), and a timestamp. As the Driver Station can only transmit enable/disable state to one robot, the slave functionally self-enables by sending enabled packets through its loopback address, based on the NT value. Telemetry-wise, robot positions, individual robot target positions, vision data, and Swerve Module positions are shown, for both the master and the slave. These can all be viewed through complex, 3d widgets, through AdvantageScope.
The final usage of NetworkTables is tuning PID gains. NT allows clients to both publish and subscribe to inputs, which allows the user to send gains through the Driver Station to the robots. This means gains can be tuned dynamically, without re-deploying code.
Pose estimation
Our pose estimation algorithm is based on WPILib’s Swerve Pose Estimator. It uses a Kalman filter to integrate timestamped vision data with odometry, by estimating the robot’s odometric position at the vision timestamp, and projecting forward to a real-time position. We modified the default pose estimator to allow for two additional features. First, the Kalman gains for the odometry (which are usually constant) now change based on deviance from vision. Second, if odometry drift is too great, it is functionally hard reset to the vision pose. These new features primarily had to be implemented because of the state of the NerdSwerves that we tested on; their gears were stripping.
Our vision system uses an Orange Pi 5+ connected to a 100MP ArduCam running PhotonVision, which grabs a camera-relative position from an apriltag mounted on the master robot. The camera-relative position is then translated to get a slave-to-master robot offset. Finally, this is translated by the master position to get a field-relative global position that can be fed into the pose estimator.
A point of improvement for the pose estimator would be using every input stream, including the master’s vision system. The reason why we chose not to implement the master into our current pose estimation scheme is because, as the slave is dependent on the master position, using the slave as a vision target might cause a feedback loop. A homemade estimator and Kalmann filter that incorporates all of these inputs, and returns system-relative positions would remove that risk.
Tandem driving
Our first tandem driving algorithm used a SwerveDriveKinematics object to simulate each NERDSwerve as a single swerve pod, with the NERDSwerve formation acting as the whole robot. Joystick input would be directly translated into a formation speed, which would then be translated into each robot’s speed using inverse kinematics. The robot speeds would be directly set to each robot with no feedback control. Because of the lack of a feedback controller, the robot formation quickly accumulated drift, even if the pose estimator remained accurate.
A global target position is calculated on the master and incremented periodically with joystick inputs. This target position is published by the master and subscribed to by the slave, allowing each robot to use its formation-relative position (functionally, an offset from the global target position) to calculate its own target position every loop cycle. A PID controller controls each robot’s output speeds in the X, Y, and rotational directions, resulting in the robots holding a set formation and responding to adjustments in the formation’s target position.
Relying on a PID controller to handle robot movement results in input lag and can cause the robots to continue moving after the joystick is released, so this algorithm is an imperfect solution. To improve responsiveness and controllability, we combined a speed-based joystick input command with a formation-holding PID loop to set robot speeds, which would eliminate input lag and retain the robots’ ability to maintain formation. However, we only implemented the formation-holding PID controller on the slave robot, or the formation could experience drift and oscillation as each robot attempts to “catch up” with the other.
Formation Adjustment
We thought it would be useful to have the ability to quickly rearrange the robots’ formation, so we added a second command that allows the user to control each robot independently. When the user switches to the independent driving command, each robot can be moved freely by the joysticks or by someone picking them up. When the user switches back to the tandem driving command, the slave saves its current position relative to the master as its new offset and the PID controller is reengaged so that the slave follows that offset. Because the robots are in a new formation, the inverse kinematics that are used to calculate each robot’s speed based on the joystick input are also adjusted.
