Use Kalman filter for IR sensor instead of taking several measurements and averaging. Floats should be replaced with fixed-point calculations