Finished math.
Final test with hardware. Working.