Working code

Might be some bugs if battery voltage is between charging voltage and LV
disconnect voltage. More testing is needed.