Skip to content

Instantly share code, notes, and snippets.

@Kdoje Kdoje/Question1.m
Created Nov 12, 2018

Embed
What would you like to do?
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]
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.