[Ether]

I threw this together quickly. Can you guys critique it? Maybe someone can debug it on real hardware, or in a simulation.

double change, PosCmd, error1, e1pos, error1i, 
error1d, cmd1, error2a, e2pos, cmd2a, 
error2b, error2bi, cmd2b, cmd2;

// initialize these:

double RLPC = 0;  // Rate-Limited Position Command
double previous_error1 = 0;

// tuning constants:

double rateLimit = 1;  // allowable position change per iteration
double i1Limit = 1;     // integrator clamp for M1
double i2bLimit = 1;   // integrator clamping value for M2
double Kp1 = 0;  // PID gains
double Ki1 = 0;
double Kd1 = 0;
double Kp2 = 0;
double Ki2 = 0;


// this is your control loop:

// rate-limit the change in position command

change = PosCmd - RLPC;
if (change>rateLimit) change = rateLimit;
else if (change<-rateLimit) change = -rateLimit;
RLPC += change;

// PID controller for M1 (Master)

error1 = RLPC - e1pos;  // closed loop error
error1i += error1;  // integrate the error
if (error1i>i1Limit) error1i = i1Limit;  // clamp the integrated error
else if (error1i<-i1Limit) error1i = -i1Limit;
error1d = error1 - previous_error1; // rate of change in error1 for D term
previous_error1 = error1; // save for next iteration
cmd1 = Kp1*error1 + Ki1*error1i + Kd1*error1d;

// P + I controller for M2 (slave)

error2a = RLPC - e2pos;  // closed loop error for Proportional part
cmd2a = Kp2*error2a;

error2b = e1pos - e2pos;  // closed loop error for Integral part
error2bi += error2b;  // integrate the error
if (error2bi>i2bLimit) error2bi = i2bLimit;  // clamp the integrated error
else if (error2bi<-i2bLimit) error2bi = -i2bLimit;
cmd2b = Ki2*error2bi;

cmd2 = cmd2a + cmd2b;