Zeroed some variables at initialization
Added torque saturation on torqueBalancing