Hi, Im Electronics Engineer, with over 8 years of Experince, i belive i can solve your problem,
i just gothrough your program and see you are using ADC via timer and calculate things,
and other is directly without inttrupt,
i have two possible suggessions/solutions in mind, let me know if intrested,