Arduino sketch mostly done. Need to test it with servo's.