Fixed reading gains in simulation + Reading encoder offsets and generating tau values. Write USB packets still doesn't work