I think you really need a better control algorithm. What have now is a fairly simple first order system which is bound to have a static error the size of which depends primarily on the loop gain. To ensure the measured current more clasely equals the desired current you probably need to move to a second oder (PI) or third oder (PID) system. I think you will find info on coding these on other threads on this site.

Ian