You might want to look at my
De-coupling tutorial
The Pi filters at the end are good for stopping the noise from motors spreading through your system.
You can not code your way out of an inadequate power supply. Nether can you get round it using capacitors.