Well, i'm really not a Motor Driver pro... so i would suggest you to increase the pulse length and see if it does anything better.

40mA measurement is not revealant, it's pulse measurement, it give an average of the whole thing, not the peak.. unless you're using something else fancier than a regular DVM.

6 Volts to a regular LM7805 input may cause stability problem, if my memory serves me well, Min Vin should be 'round 9 Volts.