Finally I come with a fully functional PID algorithm ready to rock :)

Here you can see an acceleration/breaking ramp test, of course you can adjust the response by tuning the gains (Kp, Kd, Ki) to your own preferences. Slow responses may be useful to avoid stripped gears at sudden movements, also when you have a tall inestable robot that can flip upside down. And when you have a stable robot you can reduce response time to have faster movement transitions. A perfect gain adjust will let you have the best of both worlds. 

Remember that all the code is available on GitHub, I just used the standard PID equation with some tricks to scale the values to proper ones for backward and forward rotation with same equation. Not so much an interesting thing to discuss ;)

void calculate_pidM(int set) //PID calculation function
{
    float error = 0;
    float pid = 0;
    error = set - I2C_buffer.data.RPM; //calculate actual error
    pid = error * I2C_buffer.data.RPM_PID_KP; // calculate proportional gain
    accumulatorM += error; // calculate accumulator, is sum of errors
    pid += I2C_buffer.data.RPM_PID_KI*accumulatorM; // add integral gain and error accumulator
    pid += I2C_buffer.data.RPM_PID_KD * (error - lasterrorM); //add differential gain
    lasterrorM = error; //save the error to the next iteration
    if (pid >= 1023) //next we guarantee that the PID value is in range
    {
        pid = 1023;
    }
    if (pid <= -1023) {
        pid = -1023;
    }
    M_control((int) pid);
}

In the next post I will publish about the auto-stop at defined distance mathematics starting from the wheel diameter and gear ratio.