// Inputs
goalPosition = 0; // deg
deadZone = 0; // deg
saturatorOffset = 64; // PWM
saturatorSlope = 5; // PWM/deg
PWMOffset = 0; // PWM
minPWM = 0; // PWM
maxPWM = 550; // PWM


// Evaluate size x and y axis
xExtra = 50; // deg
xPrecision = 0.1; // deg
xExtension = deadZone + (maxPWM - saturatorOffset)/saturatorSlope + xExtra;
xMax = goalPosition + xExtension;
xMin = goalPosition - xExtension;
X = xMin:xPrecision:xMax;

Y = zeros(X);
50
i=1;
for x=X
    dist = goalPosition-x;
    if(dist == 0)
        Y(1,i) = PWMOffset;
    elseif(abs(dist) < deadZone)
        Y(1,i) = PWMOffset + sign(dist)*minPWM;
    elseif(abs(dist) < xExtension - xExtra)
        Y(1,i) = PWMOffset + sign(dist)*saturatorOffset + (dist-sign(dist)*deadZone)*saturatorSlope;
    else
        Y(1,i) = PWMOffset + sign(dist)*maxPWM;
    end
    i = i+1;
end

clf();
plot(X,Y);
