close all; clear; clc;
global debugLevel_01;
debugLevel_01 = 1;
L1 = 110.0;
L2 = 140.0;
initalizeArmCoordinates;
printArmCoordinates;
hf = figure('color','white');
axis equal
SUM_L1_L2 = L1 + L2;
maximum_x_limit = ceil(SUM_L1_L2/100)*100;
maximum_y_limit = ceil(SUM_L1_L2/100)*100;
minimum_x_limit = - ceil(SUM_L1_L2/100)*100;
minimum_y_limit = - ceil(SUM_L1_L2/100)*100;
xlim([minimum_x_limit maximum_x_limit])
ylim([minimum_y_limit maximum_y_limit])
set(gca,'XTick',minimum_x_limit : 100 : maximum_x_limit);
set(gca,'YTick',minimum_y_limit : 100 : maximum_y_limit);
grid on
set(gca, 'GridLineStyle', '-');
grid(gca,'minor')
title('Animate a rotating robotic arm','Color',[0 0 1])
hold on
ht(1) = plot(lx(1:2), ly(1:2), 'color', [.4 .4 .8],'LineWidth',3);
ht(2) = plot(lx(2:3), ly(2:3), 'color', [.8 .4 .8],'LineWidth',3);
ht(3) = plot(lx(1),ly(1),'--mo', 'MarkerEdgeColor','k', 'MarkerFaceColor',[.49 1 .63], 'MarkerSize',6);
ht(4) = plot(lx(2),ly(2),'--mo', 'MarkerEdgeColor','k', 'MarkerFaceColor',[.99 1 .63], 'MarkerSize',6);
ht(5) = plot(lx(3),ly(3),'--mo', 'MarkerEdgeColor','k', 'MarkerFaceColor',[.49 0 .63], 'MarkerSize',6);
pause on;
degree = -1;
motor_number = 1;
for frame_no = 1:90
[ successFlag, lx, ly ] = rotate_motor_logically( motor_number, degree, lx, ly, debugLevel_01);
set(ht(1),'XData',lx(1:2)); set(ht(1),'YData',ly(1:2));
set(ht(2),'XData',lx(2:3)); set(ht(2),'YData',ly(2:3));
set(ht(3),'XData',lx(1)); set(ht(3),'YData',ly(1));
set(ht(4),'XData',lx(2)); set(ht(4),'YData',ly(2));
set(ht(5),'XData',lx(3)); set(ht(5),'YData',ly(3));
drawnow;
pause (0.1);
end
saveas(gcf,'output.jpg');
lx[1] = 0.00 lx[2] = 110.00 lx[3] = 110.00
ly[1] = 0.00 ly[2] = 0.00 ly[3] = 140.00
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 110.00 lx[3] = 110.00
DEBUG: ly[1] = 0.00 ly[2] = 0.00 ly[3] = 140.00
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.98 nlx[3] = 107.54
DEBUG: nly[1] = 0.00 nly[2] = 1.92 nly[3] = 141.90
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.98 lx[3] = 107.54
DEBUG: ly[1] = 0.00 ly[2] = 1.92 ly[3] = 141.90
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.93 nlx[3] = 105.05
DEBUG: nly[1] = 0.00 nly[2] = 3.84 nly[3] = 143.75
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.93 lx[3] = 105.05
DEBUG: ly[1] = 0.00 ly[2] = 3.84 ly[3] = 143.75
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.85 nlx[3] = 102.52
DEBUG: nly[1] = 0.00 nly[2] = 5.76 nly[3] = 145.57
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.85 lx[3] = 102.52
DEBUG: ly[1] = 0.00 ly[2] = 5.76 ly[3] = 145.57
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.73 nlx[3] = 99.97
DEBUG: nly[1] = 0.00 nly[2] = 7.67 nly[3] = 147.33
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.73 lx[3] = 99.97
DEBUG: ly[1] = 0.00 ly[2] = 7.67 ly[3] = 147.33
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.58 nlx[3] = 97.38
DEBUG: nly[1] = 0.00 nly[2] = 9.59 nly[3] = 149.05
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.58 lx[3] = 97.38
DEBUG: ly[1] = 0.00 ly[2] = 9.59 ly[3] = 149.05
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.40 nlx[3] = 94.76
DEBUG: nly[1] = 0.00 nly[2] = 11.50 nly[3] = 150.73
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.40 lx[3] = 94.76
DEBUG: ly[1] = 0.00 ly[2] = 11.50 ly[3] = 150.73
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 109.18 nlx[3] = 92.12
DEBUG: nly[1] = 0.00 nly[2] = 13.41 nly[3] = 152.36
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 109.18 lx[3] = 92.12
DEBUG: ly[1] = 0.00 ly[2] = 13.41 ly[3] = 152.36
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 108.93 nlx[3] = 89.45
DEBUG: nly[1] = 0.00 nly[2] = 15.31 nly[3] = 153.95
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 108.93 lx[3] = 89.45
DEBUG: ly[1] = 0.00 ly[2] = 15.31 ly[3] = 153.95
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 108.65 nlx[3] = 86.74
DEBUG: nly[1] = 0.00 nly[2] = 17.21 nly[3] = 155.48
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 108.65 lx[3] = 86.74
DEBUG: ly[1] = 0.00 ly[2] = 17.21 ly[3] = 155.48
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 108.33 nlx[3] = 84.02
DEBUG: nly[1] = 0.00 nly[2] = 19.10 nly[3] = 156.97
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 108.33 lx[3] = 84.02
DEBUG: ly[1] = 0.00 ly[2] = 19.10 ly[3] = 156.97
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 107.98 nlx[3] = 81.27
DEBUG: nly[1] = 0.00 nly[2] = 20.99 nly[3] = 158.42
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 107.98 lx[3] = 81.27
DEBUG: ly[1] = 0.00 ly[2] = 20.99 ly[3] = 158.42
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 107.60 nlx[3] = 78.49
DEBUG: nly[1] = 0.00 nly[2] = 22.87 nly[3] = 159.81
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 107.60 lx[3] = 78.49
DEBUG: ly[1] = 0.00 ly[2] = 22.87 ly[3] = 159.81
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 107.18 nlx[3] = 75.69
DEBUG: nly[1] = 0.00 nly[2] = 24.74 nly[3] = 161.16
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 107.18 lx[3] = 75.69
DEBUG: ly[1] = 0.00 ly[2] = 24.74 ly[3] = 161.16
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 106.73 nlx[3] = 72.86
DEBUG: nly[1] = 0.00 nly[2] = 26.61 nly[3] = 162.45
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 106.73 lx[3] = 72.86
DEBUG: ly[1] = 0.00 ly[2] = 26.61 ly[3] = 162.45
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 106.25 nlx[3] = 70.02
DEBUG: nly[1] = 0.00 nly[2] = 28.47 nly[3] = 163.70
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 106.25 lx[3] = 70.02
DEBUG: ly[1] = 0.00 ly[2] = 28.47 ly[3] = 163.70
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 105.74 nlx[3] = 67.15
DEBUG: nly[1] = 0.00 nly[2] = 30.32 nly[3] = 164.90
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 105.74 lx[3] = 67.15
DEBUG: ly[1] = 0.00 ly[2] = 30.32 ly[3] = 164.90
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 105.19 nlx[3] = 64.26
DEBUG: nly[1] = 0.00 nly[2] = 32.16 nly[3] = 166.04
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 105.19 lx[3] = 64.26
DEBUG: ly[1] = 0.00 ly[2] = 32.16 ly[3] = 166.04
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 104.62 nlx[3] = 61.35
DEBUG: nly[1] = 0.00 nly[2] = 33.99 nly[3] = 167.14
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 104.62 lx[3] = 61.35
DEBUG: ly[1] = 0.00 ly[2] = 33.99 ly[3] = 167.14
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 104.01 nlx[3] = 58.43
DEBUG: nly[1] = 0.00 nly[2] = 35.81 nly[3] = 168.19
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 104.01 lx[3] = 58.43
DEBUG: ly[1] = 0.00 ly[2] = 35.81 ly[3] = 168.19
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 103.37 nlx[3] = 55.48
DEBUG: nly[1] = 0.00 nly[2] = 37.62 nly[3] = 169.18
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 103.37 lx[3] = 55.48
DEBUG: ly[1] = 0.00 ly[2] = 37.62 ly[3] = 169.18
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 102.69 nlx[3] = 52.52
DEBUG: nly[1] = 0.00 nly[2] = 39.42 nly[3] = 170.12
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 102.69 lx[3] = 52.52
DEBUG: ly[1] = 0.00 ly[2] = 39.42 ly[3] = 170.12
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 101.99 nlx[3] = 49.55
DEBUG: nly[1] = 0.00 nly[2] = 41.21 nly[3] = 171.01
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 101.99 lx[3] = 49.55
DEBUG: ly[1] = 0.00 ly[2] = 41.21 ly[3] = 171.01
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 101.26 nlx[3] = 46.55
DEBUG: nly[1] = 0.00 nly[2] = 42.98 nly[3] = 171.85
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 101.26 lx[3] = 46.55
DEBUG: ly[1] = 0.00 ly[2] = 42.98 ly[3] = 171.85
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 100.49 nlx[3] = 43.55
DEBUG: nly[1] = 0.00 nly[2] = 44.74 nly[3] = 172.64
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 100.49 lx[3] = 43.55
DEBUG: ly[1] = 0.00 ly[2] = 44.74 ly[3] = 172.64
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 99.69 nlx[3] = 40.53
DEBUG: nly[1] = 0.00 nly[2] = 46.49 nly[3] = 173.37
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 99.69 lx[3] = 40.53
DEBUG: ly[1] = 0.00 ly[2] = 46.49 ly[3] = 173.37
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 98.87 nlx[3] = 37.50
DEBUG: nly[1] = 0.00 nly[2] = 48.22 nly[3] = 174.05
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 98.87 lx[3] = 37.50
DEBUG: ly[1] = 0.00 ly[2] = 48.22 ly[3] = 174.05
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 98.01 nlx[3] = 34.45
DEBUG: nly[1] = 0.00 nly[2] = 49.94 nly[3] = 174.68
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 98.01 lx[3] = 34.45
DEBUG: ly[1] = 0.00 ly[2] = 49.94 ly[3] = 174.68
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 97.12 nlx[3] = 31.40
DEBUG: nly[1] = 0.00 nly[2] = 51.64 nly[3] = 175.25
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 97.12 lx[3] = 31.40
DEBUG: ly[1] = 0.00 ly[2] = 51.64 ly[3] = 175.25
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 96.21 nlx[3] = 28.33
DEBUG: nly[1] = 0.00 nly[2] = 53.33 nly[3] = 175.78
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 96.21 lx[3] = 28.33
DEBUG: ly[1] = 0.00 ly[2] = 53.33 ly[3] = 175.78
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 95.26 nlx[3] = 25.26
DEBUG: nly[1] = 0.00 nly[2] = 55.00 nly[3] = 176.24
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 95.26 lx[3] = 25.26
DEBUG: ly[1] = 0.00 ly[2] = 55.00 ly[3] = 176.24
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 94.29 nlx[3] = 22.18
DEBUG: nly[1] = 0.00 nly[2] = 56.65 nly[3] = 176.66
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 94.29 lx[3] = 22.18
DEBUG: ly[1] = 0.00 ly[2] = 56.65 ly[3] = 176.66
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 93.29 nlx[3] = 19.10
DEBUG: nly[1] = 0.00 nly[2] = 58.29 nly[3] = 177.02
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 93.29 lx[3] = 19.10
DEBUG: ly[1] = 0.00 ly[2] = 58.29 ly[3] = 177.02
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 92.25 nlx[3] = 16.00
DEBUG: nly[1] = 0.00 nly[2] = 59.91 nly[3] = 177.32
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 92.25 lx[3] = 16.00
DEBUG: ly[1] = 0.00 ly[2] = 59.91 ly[3] = 177.32
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 91.19 nlx[3] = 12.91
DEBUG: nly[1] = 0.00 nly[2] = 61.51 nly[3] = 177.58
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 91.19 lx[3] = 12.91
DEBUG: ly[1] = 0.00 ly[2] = 61.51 ly[3] = 177.58
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 90.11 nlx[3] = 9.81
DEBUG: nly[1] = 0.00 nly[2] = 63.09 nly[3] = 177.77
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 90.11 lx[3] = 9.81
DEBUG: ly[1] = 0.00 ly[2] = 63.09 ly[3] = 177.77
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 88.99 nlx[3] = 6.70
DEBUG: nly[1] = 0.00 nly[2] = 64.66 nly[3] = 177.92
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 88.99 lx[3] = 6.70
DEBUG: ly[1] = 0.00 ly[2] = 64.66 ly[3] = 177.92
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 87.85 nlx[3] = 3.60
DEBUG: nly[1] = 0.00 nly[2] = 66.20 nly[3] = 178.01
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 87.85 lx[3] = 3.60
DEBUG: ly[1] = 0.00 ly[2] = 66.20 ly[3] = 178.01
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 86.68 nlx[3] = 0.49
DEBUG: nly[1] = 0.00 nly[2] = 67.72 nly[3] = 178.04
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 86.68 lx[3] = 0.49
DEBUG: ly[1] = 0.00 ly[2] = 67.72 ly[3] = 178.04
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 85.49 nlx[3] = -2.62
DEBUG: nly[1] = 0.00 nly[2] = 69.23 nly[3] = 178.03
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 85.49 lx[3] = -2.62
DEBUG: ly[1] = 0.00 ly[2] = 69.23 ly[3] = 178.03
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 84.26 nlx[3] = -5.73
DEBUG: nly[1] = 0.00 nly[2] = 70.71 nly[3] = 177.95
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 84.26 lx[3] = -5.73
DEBUG: ly[1] = 0.00 ly[2] = 70.71 ly[3] = 177.95
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 83.02 nlx[3] = -8.83
DEBUG: nly[1] = 0.00 nly[2] = 72.17 nly[3] = 177.83
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 83.02 lx[3] = -8.83
DEBUG: ly[1] = 0.00 ly[2] = 72.17 ly[3] = 177.83
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 81.75 nlx[3] = -11.93
DEBUG: nly[1] = 0.00 nly[2] = 73.60 nly[3] = 177.64
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 81.75 lx[3] = -11.93
DEBUG: ly[1] = 0.00 ly[2] = 73.60 ly[3] = 177.64
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 80.45 nlx[3] = -15.03
DEBUG: nly[1] = 0.00 nly[2] = 75.02 nly[3] = 177.41
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 80.45 lx[3] = -15.03
DEBUG: ly[1] = 0.00 ly[2] = 75.02 ly[3] = 177.41
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 79.13 nlx[3] = -18.12
DEBUG: nly[1] = 0.00 nly[2] = 76.41 nly[3] = 177.12
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 79.13 lx[3] = -18.12
DEBUG: ly[1] = 0.00 ly[2] = 76.41 ly[3] = 177.12
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 77.78 nlx[3] = -21.21
DEBUG: nly[1] = 0.00 nly[2] = 77.78 nly[3] = 176.78
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 77.78 lx[3] = -21.21
DEBUG: ly[1] = 0.00 ly[2] = 77.78 ly[3] = 176.78
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 76.41 nlx[3] = -24.30
DEBUG: nly[1] = 0.00 nly[2] = 79.13 nly[3] = 176.38
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 76.41 lx[3] = -24.30
DEBUG: ly[1] = 0.00 ly[2] = 79.13 ly[3] = 176.38
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 75.02 nlx[3] = -27.37
DEBUG: nly[1] = 0.00 nly[2] = 80.45 nly[3] = 175.93
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 75.02 lx[3] = -27.37
DEBUG: ly[1] = 0.00 ly[2] = 80.45 ly[3] = 175.93
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 73.60 nlx[3] = -30.44
DEBUG: nly[1] = 0.00 nly[2] = 81.75 nly[3] = 175.42
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 73.60 lx[3] = -30.44
DEBUG: ly[1] = 0.00 ly[2] = 81.75 ly[3] = 175.42
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 72.17 nlx[3] = -33.49
DEBUG: nly[1] = 0.00 nly[2] = 83.02 nly[3] = 174.87
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 72.17 lx[3] = -33.49
DEBUG: ly[1] = 0.00 ly[2] = 83.02 ly[3] = 174.87
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 70.71 nlx[3] = -36.54
DEBUG: nly[1] = 0.00 nly[2] = 84.26 nly[3] = 174.26
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 70.71 lx[3] = -36.54
DEBUG: ly[1] = 0.00 ly[2] = 84.26 ly[3] = 174.26
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 69.23 nlx[3] = -39.58
DEBUG: nly[1] = 0.00 nly[2] = 85.49 nly[3] = 173.59
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 69.23 lx[3] = -39.58
DEBUG: ly[1] = 0.00 ly[2] = 85.49 ly[3] = 173.59
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 67.72 nlx[3] = -42.60
DEBUG: nly[1] = 0.00 nly[2] = 86.68 nly[3] = 172.87
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 67.72 lx[3] = -42.60
DEBUG: ly[1] = 0.00 ly[2] = 86.68 ly[3] = 172.87
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 66.20 nlx[3] = -45.61
DEBUG: nly[1] = 0.00 nly[2] = 87.85 nly[3] = 172.10
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 66.20 lx[3] = -45.61
DEBUG: ly[1] = 0.00 ly[2] = 87.85 ly[3] = 172.10
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 64.66 nlx[3] = -48.61
DEBUG: nly[1] = 0.00 nly[2] = 88.99 nly[3] = 171.28
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 64.66 lx[3] = -48.61
DEBUG: ly[1] = 0.00 ly[2] = 88.99 ly[3] = 171.28
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 63.09 nlx[3] = -51.59
DEBUG: nly[1] = 0.00 nly[2] = 90.11 nly[3] = 170.41
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 63.09 lx[3] = -51.59
DEBUG: ly[1] = 0.00 ly[2] = 90.11 ly[3] = 170.41
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 61.51 nlx[3] = -54.55
DEBUG: nly[1] = 0.00 nly[2] = 91.19 nly[3] = 169.48
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 61.51 lx[3] = -54.55
DEBUG: ly[1] = 0.00 ly[2] = 91.19 ly[3] = 169.48
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 59.91 nlx[3] = -57.50
DEBUG: nly[1] = 0.00 nly[2] = 92.25 nly[3] = 168.50
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 59.91 lx[3] = -57.50
DEBUG: ly[1] = 0.00 ly[2] = 92.25 ly[3] = 168.50
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 58.29 nlx[3] = -60.44
DEBUG: nly[1] = 0.00 nly[2] = 93.29 nly[3] = 167.47
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 58.29 lx[3] = -60.44
DEBUG: ly[1] = 0.00 ly[2] = 93.29 ly[3] = 167.47
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 56.65 nlx[3] = -63.35
DEBUG: nly[1] = 0.00 nly[2] = 94.29 nly[3] = 166.39
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 56.65 lx[3] = -63.35
DEBUG: ly[1] = 0.00 ly[2] = 94.29 ly[3] = 166.39
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 55.00 nlx[3] = -66.24
DEBUG: nly[1] = 0.00 nly[2] = 95.26 nly[3] = 165.26
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 55.00 lx[3] = -66.24
DEBUG: ly[1] = 0.00 ly[2] = 95.26 ly[3] = 165.26
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 53.33 nlx[3] = -69.12
DEBUG: nly[1] = 0.00 nly[2] = 96.21 nly[3] = 164.08
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 53.33 lx[3] = -69.12
DEBUG: ly[1] = 0.00 ly[2] = 96.21 ly[3] = 164.08
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 51.64 nlx[3] = -71.97
DEBUG: nly[1] = 0.00 nly[2] = 97.12 nly[3] = 162.85
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 51.64 lx[3] = -71.97
DEBUG: ly[1] = 0.00 ly[2] = 97.12 ly[3] = 162.85
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 49.94 nlx[3] = -74.80
DEBUG: nly[1] = 0.00 nly[2] = 98.01 nly[3] = 161.57
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 49.94 lx[3] = -74.80
DEBUG: ly[1] = 0.00 ly[2] = 98.01 ly[3] = 161.57
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 48.22 nlx[3] = -77.61
DEBUG: nly[1] = 0.00 nly[2] = 98.87 nly[3] = 160.24
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 48.22 lx[3] = -77.61
DEBUG: ly[1] = 0.00 ly[2] = 98.87 ly[3] = 160.24
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 46.49 nlx[3] = -80.40
DEBUG: nly[1] = 0.00 nly[2] = 99.69 nly[3] = 158.86
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 46.49 lx[3] = -80.40
DEBUG: ly[1] = 0.00 ly[2] = 99.69 ly[3] = 158.86
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 44.74 nlx[3] = -83.16
DEBUG: nly[1] = 0.00 nly[2] = 100.49 nly[3] = 157.43
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 44.74 lx[3] = -83.16
DEBUG: ly[1] = 0.00 ly[2] = 100.49 ly[3] = 157.43
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 42.98 nlx[3] = -85.89
DEBUG: nly[1] = 0.00 nly[2] = 101.26 nly[3] = 155.96
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 42.98 lx[3] = -85.89
DEBUG: ly[1] = 0.00 ly[2] = 101.26 ly[3] = 155.96
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 41.21 nlx[3] = -88.60
DEBUG: nly[1] = 0.00 nly[2] = 101.99 nly[3] = 154.44
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 41.21 lx[3] = -88.60
DEBUG: ly[1] = 0.00 ly[2] = 101.99 ly[3] = 154.44
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 39.42 nlx[3] = -91.28
DEBUG: nly[1] = 0.00 nly[2] = 102.69 nly[3] = 152.87
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 39.42 lx[3] = -91.28
DEBUG: ly[1] = 0.00 ly[2] = 102.69 ly[3] = 152.87
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 37.62 nlx[3] = -93.93
DEBUG: nly[1] = 0.00 nly[2] = 103.37 nly[3] = 151.25
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 37.62 lx[3] = -93.93
DEBUG: ly[1] = 0.00 ly[2] = 103.37 ly[3] = 151.25
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 35.81 nlx[3] = -96.56
DEBUG: nly[1] = 0.00 nly[2] = 104.01 nly[3] = 149.59
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 35.81 lx[3] = -96.56
DEBUG: ly[1] = 0.00 ly[2] = 104.01 ly[3] = 149.59
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 33.99 nlx[3] = -99.16
DEBUG: nly[1] = 0.00 nly[2] = 104.62 nly[3] = 147.88
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 33.99 lx[3] = -99.16
DEBUG: ly[1] = 0.00 ly[2] = 104.62 ly[3] = 147.88
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 32.16 nlx[3] = -101.72
DEBUG: nly[1] = 0.00 nly[2] = 105.19 nly[3] = 146.13
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 32.16 lx[3] = -101.72
DEBUG: ly[1] = 0.00 ly[2] = 105.19 ly[3] = 146.13
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 30.32 nlx[3] = -104.26
DEBUG: nly[1] = 0.00 nly[2] = 105.74 nly[3] = 144.33
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 30.32 lx[3] = -104.26
DEBUG: ly[1] = 0.00 ly[2] = 105.74 ly[3] = 144.33
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 28.47 nlx[3] = -106.76
DEBUG: nly[1] = 0.00 nly[2] = 106.25 nly[3] = 142.49
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 28.47 lx[3] = -106.76
DEBUG: ly[1] = 0.00 ly[2] = 106.25 ly[3] = 142.49
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 26.61 nlx[3] = -109.23
DEBUG: nly[1] = 0.00 nly[2] = 106.73 nly[3] = 140.60
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 26.61 lx[3] = -109.23
DEBUG: ly[1] = 0.00 ly[2] = 106.73 ly[3] = 140.60
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 24.74 nlx[3] = -111.67
DEBUG: nly[1] = 0.00 nly[2] = 107.18 nly[3] = 138.67
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 24.74 lx[3] = -111.67
DEBUG: ly[1] = 0.00 ly[2] = 107.18 ly[3] = 138.67
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 22.87 nlx[3] = -114.07
DEBUG: nly[1] = 0.00 nly[2] = 107.60 nly[3] = 136.70
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 22.87 lx[3] = -114.07
DEBUG: ly[1] = 0.00 ly[2] = 107.60 ly[3] = 136.70
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 20.99 nlx[3] = -116.44
DEBUG: nly[1] = 0.00 nly[2] = 107.98 nly[3] = 134.69
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 20.99 lx[3] = -116.44
DEBUG: ly[1] = 0.00 ly[2] = 107.98 ly[3] = 134.69
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 19.10 nlx[3] = -118.77
DEBUG: nly[1] = 0.00 nly[2] = 108.33 nly[3] = 132.64
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 19.10 lx[3] = -118.77
DEBUG: ly[1] = 0.00 ly[2] = 108.33 ly[3] = 132.64
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 17.21 nlx[3] = -121.07
DEBUG: nly[1] = 0.00 nly[2] = 108.65 nly[3] = 130.55
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 17.21 lx[3] = -121.07
DEBUG: ly[1] = 0.00 ly[2] = 108.65 ly[3] = 130.55
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 15.31 nlx[3] = -123.33
DEBUG: nly[1] = 0.00 nly[2] = 108.93 nly[3] = 128.41
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 15.31 lx[3] = -123.33
DEBUG: ly[1] = 0.00 ly[2] = 108.93 ly[3] = 128.41
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 13.41 nlx[3] = -125.55
DEBUG: nly[1] = 0.00 nly[2] = 109.18 nly[3] = 126.24
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 13.41 lx[3] = -125.55
DEBUG: ly[1] = 0.00 ly[2] = 109.18 ly[3] = 126.24
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 11.50 nlx[3] = -127.73
DEBUG: nly[1] = 0.00 nly[2] = 109.40 nly[3] = 124.03
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 11.50 lx[3] = -127.73
DEBUG: ly[1] = 0.00 ly[2] = 109.40 ly[3] = 124.03
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 9.59 nlx[3] = -129.88
DEBUG: nly[1] = 0.00 nly[2] = 109.58 nly[3] = 121.78
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 9.59 lx[3] = -129.88
DEBUG: ly[1] = 0.00 ly[2] = 109.58 ly[3] = 121.78
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 7.67 nlx[3] = -131.99
DEBUG: nly[1] = 0.00 nly[2] = 109.73 nly[3] = 119.50
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 7.67 lx[3] = -131.99
DEBUG: ly[1] = 0.00 ly[2] = 109.73 ly[3] = 119.50
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 5.76 nlx[3] = -134.05
DEBUG: nly[1] = 0.00 nly[2] = 109.85 nly[3] = 117.18
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 5.76 lx[3] = -134.05
DEBUG: ly[1] = 0.00 ly[2] = 109.85 ly[3] = 117.18
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 3.84 nlx[3] = -136.08
DEBUG: nly[1] = 0.00 nly[2] = 109.93 nly[3] = 114.82
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 3.84 lx[3] = -136.08
DEBUG: ly[1] = 0.00 ly[2] = 109.93 ly[3] = 114.82
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 1.92 nlx[3] = -138.06
DEBUG: nly[1] = 0.00 nly[2] = 109.98 nly[3] = 112.43
DEBUG: Before Rotating motor 1
DEBUG: lx[1] = 0.00 lx[2] = 1.92 lx[3] = -138.06
DEBUG: ly[1] = 0.00 ly[2] = 109.98 ly[3] = 112.43
DEBUG: After Rotating motor 1 by -1.000000 degree
DEBUG: nlx[1] = 0.00 nlx[2] = 0.00 nlx[3] = -140.00
DEBUG: nly[1] = 0.00 nly[2] = 110.00 nly[3] = 110.00
