I just had a quick glance at your code. Floats are usually a reason for slowing down code a lot. I would suggest to transform your formulas to integer computations.
However your code also contains lots of LCD IO which is usually also very slow.
All of this may or may not contribute to your issue.
The first step to actual perfomance improvements is to measure why it is so slow in the first place. Read Section 1.1 and 1.2 of this General Strategies
Once you have read these two sections you might want to google for "performance optimization c" which brings lots of reasonable ideas, e.g. this http://www.eventhelix.com/RealtimeMantra/Basics/OptimizingCAndCPPCode.htm.
But first read the paragraphs on bad and good optimization strategies.