Fix mistake in the position setpoint for the second node and remove unnecessary triple position reading loop.