Skip to content

Instantly share code, notes, and snippets.

# Kdoje/Question1.m Created Nov 12, 2018

Kent Libby question 1 gist
 %% 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]
to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.