[Ether]

Yes, you want to take the four wheel speeds that are computed from the driver commands, and use each one as a setpoint for the PID (or other controller) for the corresponding wheel. The encoder signal for each wheel serves as the process variable for that wheel.

As with all closed-loop controllers, you need to make sure that your setpoint and process variable are appropriately scaled and offset so they work together properly.

I'm not a Java guru so I'll leave the Java coding questions to someone else.