height         = 0.04
width          = 0.03
ptsPerMeter    = 300
ptsPerRadian   = 10
xlocation      = 0.12
yLocation      = 0.30
space          = 0.008

xStartLoc = xlocation;

R = THE_LETTER_R(height, width, ptsPerMeter, ptsPerRadian, [xlocation, yLocation])

    xlocation = xlocation + width + space;
O = THE_LETTER_O(height, width, ptsPerMeter, ptsPerRadian, [xlocation, yLocation])

    xlocation = xlocation + width + space;
M = THE_LETTER_M(height, width, ptsPerMeter, [xlocation, yLocation])

    xlocation = xlocation + width + space;
A = THE_LETTER_A(height, width, ptsPerMeter, [xlocation, yLocation])

    xlocation = xlocation + width + space;
N = THE_LETTER_N(height, width, ptsPerMeter, [xlocation, yLocation])

LINE = StraightLine(0, 0, (xlocation + width - xStartLoc + 2*space), 0, ptsPerMeter);
LINE = LINE + [(xStartLoc - space), (yLocation + space)]

load RobotGeometry.mat Base

END = [Base/2, (yLocation + 5*space)]

xyROMANcell = {R O M A N LINE END};

noOfLetters = length(xyROMANcell);

for ii = 1:noOfLetters
  letter = xyROMANcell{ii};
  plot(letter(:, 1), -letter(:, 2));
  hold on;
end

hold off;
% axis equal ij;

Z_i = initialPosition();

radROMANcell = cell(noOfLetters);

for ii = 1:noOfLetters
  radROMANcell{ii} = xyToRadians(xyROMANcell{ii}, Z_i, Base);
end

a = arduino;
carrier = motorCarrier(a);
s = servo(carrier, 3);
pidML = pidMotor(carrier, 2, 'position', 3, [0.14 0.0 0.01]); % Modify the PID gains [Kp Ki Kd] as per your requirements
pidMR = pidMotor(carrier, 1, 'position', 3, [0.14 0.0 0.01]); % Modify the PID gains [Kp Ki Kd] as per your requirements

load ServoPositions.mat LeftMarker RightMarker NoMarker
writePosition(s, NoMarker)

for ii = 1:noOfLetters
  % Get radians for current letter
  radianList = radROMANcell{ii};
  % Move to first letter and lower marker
  moveToRadians(radianList(1, :), pidML, pidMR)
  writePosition(s, LeftMarker)
  % Move to all positions of current letter
  moveToRadians(radianList, pidML, pidMR)
  % Raise marker
  writePosition(s, NoMarker)
end

clear