I would temporarily echo your figures through the serial port to confirm you are getting and calculating what you expect.