My truck is a 87 5.0 w/ C6, has the ford racing MAF conversion kit on it.

So I've had this issue for a while and It's not nearly as bad as it was since I changed my O2 sensor, but it's still there.

I get this random rough idle while in drive/reverse which will come and go. I'll come to a stop, idle is fine for a few seconds then I feel the truck shake and feel like it's going to die but it will just idle rough for a few seconds and then clear up and go back to normal. This also happens sometimes as soon as I put it in reverse, the engine will act like it's going to die and then clear up in a few seconds or when I tap the pedal. Neutral/park none of this ever happens.

Things i've changed very recently:

New copper spark plugs, new O2 sensor (old one was probably original and completely dead), completely new stock exhaust from front to back (the old exhaust was rotten and falling off, the previous owner put headers and custom dual exhaust on that went straight back and out the sides), had the fuel pressure checked (52 psi if I remember correctly?) engine compression checked (about 150 psi in each cylinder), IAC valve is clean and works.

Pulled the codes and got:

31 egr position sensor below minimum voltage
96 fuel pump secondary circuit failure (I'm guessing this is because the side fuel tank was removed for the dual exhaust it had )

I use to have a constant rough idle under load and an issue where it would stall immediately after the first start in the morning and then start right back up after and run fine. I changed the O2 sensor and the stalling went away and the rough idle issue is better but still there.

Any suggestions would be appreciated. I know the code 31 about the egr position sensor would be the obvious place to start though, right? Lots of googling has made me suspicious of the TPS also though (which I haven't checked at all yet)