6 motor drive turning for blocks

You could save yourself some work and use the default drivetrain with a little bit of extra code. See this link: