Writing re-usable code for something like a drive train should be pretty easy if you just pay attention to how you build up the code base
1. Separate your constants from your code. Make a constants.h. Avoid magic numbers, "const RIGHT_DRIVE_SPEED_BIAS = .97" instead of throwing .97 into your function that calculates the drive train speed. Do this with speed controller ports, sensor ports, sensor center voltages (i.e. some potentiometer is at 3.423 volts when this arm is at rest), etc etc etc. Almost anything that is physically constant on the robot should be here. Why is this useful? You moved a motor? One side is a bit too fast? Replaced a pot? You change the code on 1 line and 1 line only, everything else still works perfectly.
2. Abstraction, use it. For our drive train code, the only public functions are accessible through a DriveTrainManager object. The manager has 2 DriveSide objects, each of which has a SteeringMotor, and each steering motor has its own PID class. DriveTrainManager has NO idea that SteeringMotors even exist, it only knows about the functions in DriveSide. Why? Theoretically (if we didn't do swerve drive again) the DriveTrainManager and DriveSide objects should be completely reusable, except for the functions that rotate the wheel modules obviously, if we decided to do tank drive.
3. Avoid coupled code (spaghetti code or the infamous spaghetti and meatballs code
). Your drive train should not be using code in your arm class, and visa versa, try to encapsulate each physical aspect of your robot into its own completely modular class. Then make 1 class that deals with all the interactions. Why? Say next year you don't have an arm, so you delete the arm class. Compile the code, and now you have 5 functions in your drive train that are referencing code that no longer exists, so now you have to restructure your drive train code. This can be a complete nightmare once your code base is large enough.
4. Always make a plan, but never follow it. Planning is going to make you understand the code before you even write it, but as you begin to program you're going to realize that its all wrong and restructure the code over and over. You should know each object and module in your code before you start writing anything, or else you're going to end up writing coupled code in a struggle to get thing to work.