Added cap on I term accumulator and corrected angle constraints