fixes to speedpid. Doesn't work on robot yet.
Added cubed stuff
Reverted cubed.
Fixed drive stuff

Inversions, better pid tuning (marginally)
Tested Shooter -- works in open loop