Need to check input between range and convert it if not

The following subsystem is an error function for a robot which moves in a 2D spaces. So the error is based on the euclidean distance and the orientation error, theta. What I need to do is to check whether theta input is between two values (-pi, pi) and convert it to be in the specified range. For example, 2pi would be a whole circle but since we work on -pi, pi, it would be the same as 0, 3pi would be pi, 3pi/2 would be -pi/2 and so on, This is the subsystem, i have not found any block that could do this so specifying the range in the theta properties as -pi pi is as far as i've gotten.

Answer 1

[Fangjun Jiang]

The best way is to make sure theta is within [-pi, pi]. All trigonometry functions in MATLAB/Simulink has a defined output range. You may just need to add an offset.

If theta is an input and out of your control, you can use the mod(theta, 2*pi) function. It will gave you the output range of [0,2*pi), you will then add an offset to it.

https://www.mathworks.com/help/matlab/ref/mod.html