Converting Negative Angles into Positive Orientation

[Essembly]

Good day. I am working on a self-balancing robot (SBR) project. Here is the scene and the picture:


In my code:

Code: Select all

body = sim.getObjectHandle('SBR')
body_ort = sim.getObjectOrientation(body, -1)
sigma = body_ort[3]

I am using getObjectOrientation() to get robot's orientation around z-axis. Using matlab controller, I want the robot to turn from 0 to 360 deg and more but it seems like this function return angles in negative once the robot crosses 180 deg. Here is a plot:

I want to develop a code that would take the initial orientation of the robot as 0deg and increase the angle if SBR rotates counterclockwise and negative if SBR rotates clockwise.
Thanks in advance!

[fferri]

The range of orientation is always 360 degrees.

You have to keep track of it by yourself in your controller if e.g. you want to be able to turn 540 degrees.

One way to achieve this is to eliminate any ~360 degrees discontinuities when continuously reading the angle.

[Essembly]

Good day,
Thank you very much for your post. That's what I thought. Unfortunately, my attempts of developing such an algorithm didn't succeed. Can you help me with that?

[fferri]

Something like this:

Code: Select all

function sysCall_sensing()
    local orientation=sim.getObjectOrientation(objectHandle,-1)
    local newAngle=orientation[3]
    local increment=newAngle-angle
    while math.abs(increment)>math.pi*1.5 do
        increment=increment*(1-2*math.pi/math.abs(increment))
    end
    angle=angle+increment
    print(angle)
end

[Essembly]

Omg, it works! Thank you so much :)