Data Sheet

theta was in the first quadrant, and the new theta is in the fourth quadrant, we know that the angle

transitioned from a low to high value, so decrease the turns count.

if((theta < q2min) && (thetaP > q3max)) // If 4th to 1st quadrant

Turns++; // Increment turns count

else if((thetaP < q2min) && (theta > q3max)) // If in 1st to 4th quadrant

turns --

; // Decrement t

urns count

This code takes the number of turns and the theta angle measurement, and constructs the total angle

measurement from zero, allowing for large angle values in the +/- 2,147,483,647 degree range.

if(turns >= 0)

angle = (turns * unitsFC) + theta;

else if(turns < 0)

angle = ((turns + 1) * unitsFC) - (unitsFC - theta);

Since the 0 to 359 and 359 to 0 degree crossings rely on the angle from the previous time through the

loop, the value of thetaP (theta previous) is copied from the current theta angle before the next loop

repetition.

thetaP = theta;

osition Control System

Code from the control360 function is the bare minimum, using only proportional control to get the

servo to turn to its target position. Proportional, integral, and derivative (PID) control with a position set

point that marches forward at increments controlled by ramping and speed settings is one you might find

inside abdrive360 or servo360.

nside the loop, the first step is to check the difference between the angle set point and the measured

angle.

errorAngle = targetAngle

- angle;

For proportional control, this value can be multiplied by a constant. Kp is 1 in the example program.

output = errorAngle * Kp;

This code clamps the output to the max and min pulse speed values (assuming -speed is 1500 -20 to

1500 - 220 µs and + speed is 1500 + 20 to 1500 + 220 µs. .

if(output > 200) output = 200;

if(output < -200) output = -200;

his keeps the servo pushing slightly, even when it’s close to zero degrees errorAngle. It also results

in some oscillation. For best results, tune the 30 and -30 values for your servo.

if(errorAngle > 0)

offset = 30;

else if(errorAngle < 0)

offset = -30;

else

offset = 0;