Add winch code, add ball aq code, change drivetrain to smooth mecanum, still a few things to do however.