Cleared out some commented code and added in some active balancing. Going to test on hardware. seems to work in simulation