Tested and working, ADC needs fine tuning though