You can easily run the Sailfish 5D shield firmware with that config.
Your chosen driver (be sure to download rep-g from the sailfish thing
on thingiverse) would be:
3G 5D Cupcake+ABP (Gen3AXZ, MK5/6+Gen4 extruder, Sailfish)
It should work out of the box. Then, don;t forget, use SF50 as the
generator and choose the T-O-M default profile for gcode generation.
As another note, you then need to copy correct start and end gcode to
that profile, most of us use the T-O-M codes from an SF35 profile.
Hopefully, you've got endstops set up and that would be a really nice
printing bot, I know I have 2 of them. In fact, I can't bring myself
to get rid of them because it prints so well. Small build area, but
extremely reliable. I should note they have every upgrade, printed Z
stage, printed x-y stage, stainess rods, acrylic case, etc. THe
biggest and best fix was the Twotimes Z and X-Y stages with Z being
the most important. I also use the T-O-M leadscrew motor on Z but have
done it with a normal 8mm allthread and nut.
Plus, I designed this upgrade specifically for the hardware you have
(MK6+, Gen3 with 5D)
http://www.thingiverse.com/thing:35372
In fact, in your case, you only need to upgrade X and Y motors
(probably z while you're at it), and buy 2 belts as it uses the Xbelt
and Z belt and pulleys you already own.
Hands down, the most insane upgrade you could make. Go from 10mm x
100mm and maybe 150mm tall to full 230x230x230mm build area!
Just ask Bluemetal who just got his up and running.