multiple makehgtform in a row - plotting problems

Question

0 votes

Trying to test my calculations for guidance by implementing a 2D object that will simulate the final product.

Using Dead Reckoning, I want to make the object stop 1/3 of the way from the final point. The first for loop rotates, the second one translates. The 2 first for loops work as intended. Then i start encountering issues, the object rotates differently, and when the movement starts it starts from the first position instead of the new given position.

it might look like a lot of code, but there's a lot of the code that is repetitive.

function testSim()
x = [-9  , 3, 1, 9, 1, 3, -9 ];
y = [-3,-3,-9/2, 0, 9/2, 3, 3];
g = hgtransform;
patch('XData',x,'YData',y,'FaceColor','yellow','Parent',g)
axis equal
xlim([-20 400])
ylim([-20 400])
yObject = 314;
xObject =384;
pt1 = [0 0 0];
pt2 = [384 314 0];
pos = [xObject yObject 2 2]; 
rectangle('Position',pos,'Curvature',[1 1])
rngyRobot = 20 + (40-20)*rand(1,1);
rngxRobot = 20 + (40-20)*rand(1,1);
yObject = yObject + rngyRobot;
xObject = xObject + rngxRobot;
oldAngle = 0;
newAngle = degToObjectFromRobot(yObject,0,xObject,0)/57;
------------------------------------------------------------------------------------
for t= linspace(0,1,100)
g.Matrix = makehgtform('zrotate',oldAngle + t*(newAngle-oldAngle));
drawnow
pause(0.05)
end
for t= linspace(0,1,100)
g.Matrix = makehgtform('translate',0.01 * ((pt2/3)-pt1)) * g.Matrix;
drawnow 
pause(0.05)
end
------------------------------------------------------------------------------------
pt3 = [(384/3)  (314/3) 0];
pt2 = [384 314 0];
yObject = 314;
xObject =384;
rngyRobot = 20 + (40-20)*rand(1,1);
rngxRobot = 20 + (40-20)*rand(1,1);
yObject = yObject + rngyRobot;
xObject = xObject + rngxRobot;
oldAngle= newAngle;
newAngle = degToObjectFromRobot(yObject,yObject/3,xObject,xObject/3)/57;
------------------------------------------------------------------------------------
for t= linspace(0,1,20)
g.Matrix = makehgtform('zrotate',oldAngle + t*(newAngle-oldAngle))* g.Matrix;
drawnow
pause(0.05)
end
for t= linspace(0,1,100)
g.Matrix = makehgtform('translate',0.01 * ((pt2/1.5)-(pt3))) * g.Matrix;
drawnow 
pause(0.05)
end
------------------------------------------------------------------------------------
pt4 = [(384/1.5)  (314/1.5) 0];   
yObject = 314;
xObject =384;
rngyRobot = 20 + (40-20)*rand(1,1);
rngxRobot = 20 + (40-20)*rand(1,1);
yObject = yObject + rngyRobot;
xObject = xObject + rngxRobot;
oldAngle= newAngle;
newAngle = degToObjectFromRobot(yObject,yObject/1.5,xObject,xObject/1.5)/57;
------------------------------------------------------------------------------------
for t= linspace(0,1,20)
g.Matrix = makehgtform('zrotate',oldAngle + t*(newAngle-oldAngle));
drawnow
pause(0.05)
end
for t= linspace(0,1,100)
g.Matrix = makehgtform('translate',0.01 * ((pt2)-(pt4))) * g.Matrix;
drawnow 
pause(0.05)
end
end