If your compiler supports the REMAINDER function "x REM y" per IEC 60559 as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3 (which java does I believe), then the following single line of code should work:
Code:
SetPoint = P + Math.IEEEremainder(T-P,360.0);
or in native units:
Code:
SetPoint = P + Math.IEEEremainder(T-P,1024.0);