[Tom Bottiglieri]

Try to organize data in predefined structures and write generic functions that do work on those structures. That way if you would like to create a new instance of *something*, you can do it with limited headache. This worked well for us with things like PID control loops.

typedef struct{
  int	dState,      	// Last position input
	iState,      	// Integrator state
	iMax, 
	iMin,  	        // Maximum and minimum allowable integrator state

	iGain,    	// integral gain
        pGain,    	// proportional gain
        dGain;     	// derivative gain

  unsigned char mode1; //0 for position, 1 for velocity

} PID;

And then you can use the data like this

//Global
PID shoulder;
PID wrist;

//Function Call
void SetUpMotor(){
   InitPID(&shoulder,p,i,d,imin,imax);
   InitPID(&wrist,p2,i2,d2,imin2,imax2);
{
    
    
//Function Definition
void InitPID(PID *pid,int p, int i, int d, int i_min, int i_max)
{
    pid->pGain = p;
    pid->iGain = i;
    pid->dGain = d;
    pid->iMin=i_min;
    pid->iMax=i_max;
}

Notice how we created 2 separate PID loops with minimal code. This mimics object oriented languages like C++ or Java.
Be very careful with pointers. If you accidentally write over a reference and not a value, you will get undefined behavior out of your program (no compiler error or runtime error, just lots of weird stuff happening)