PD-algorithm working well

*Changed int abs() to double fabs()
*Increased speed of turning correction
*Increased speed of sidestepp correction
*Increased Maximum correction constant