Kent Libby question 1 gist

Question1.m

%% Kent Libby question 1 matlab script
L=.2; % in meters
end_point=[6 7]; % in meters
R=5; % in meters
D=sqrt(end_point(1)^2+end_point(2)^2)
%% This calculates angle of the arc the robot travels
arc=acos((-D^2+R^2+R^2)/(2*R*R)) % in radians
dt=20 % in seconds
omega=-arc/dt
%% This finds the wheel speeds based off radius and omega
Vr=omega*(R+L/2)
Vl=omega*(R-L/2)

%% calculate the new pose
init_x=0; % in meters
init_y=0; % in meters

tx_mat=[cos(omega*dt) -sin(omega*dt) 0;...
        sin(omega*dt) cos(omega*dt) 0;...
        0 0 1]
syms ICCx ICCy
%% Solve for ICCx and ICCy using the equation of the circle
x1=0; y1=0; x2=6; y2=7;
[solx,soly] = solve((0-ICCx)^2+(0-ICCy)^2==5^2, (6-ICCx)^2+(7-ICCy)^2==5^2);
clear ICCx ICCy
%% pick the first solution and find the ICC based off tha
ICCx=(double(solx(1)))
ICCy=(double(soly(1)))
theta=asin((ICCx-x1)/-R); % in radians
ICC=[ICCx; ICCy];
%% display the new pose
newPose=tx_mat*[init_x-ICC(1); init_y-ICC(2); theta]+[ICC; omega*dt]