Skip to content

Commit fba82ee

Browse files
VivaanBahlVivaanBahl
authored
Merge pull request #458 from CMU-Robotics-Club/vivaan-newAutonImplementation
Raceday 2017 - Fully Autonomous
2 parents e0913c9 + 4fe136c commit fba82ee

File tree

76 files changed

+7687
-282
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

76 files changed

+7687
-282
lines changed

‎offline/controller/.gitignore‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
*.mat
2+
*.jpg
3+
*.png
4+
.DS_Store
5+
*.pyc
6+

‎offline/controller/controller.m‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
function [trajectory] = controller()
2+
3+
addpath('../localizer/latlonutm/Codes/matlab');
4+
global wheel_base
5+
global velocity
6+
global steering_vel
7+
global dt
8+
9+
save_data = true;
10+
wheel_base = 1.13;
11+
utm_zone = 17;
12+
first_heading = deg2rad(250);
13+
lat_long = [40.441670, -79.9416362];
14+
dt = 0.001; % 1000Hz
15+
m = 50; % 20Hz
16+
velocity = 8; % m/s, 17.9mph, forward velocity
17+
steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds
18+
19+
[x, y, ~] = ll2utm(lat_long(1), lat_long(2));
20+
21+
X = [x; % X, m, UTM coors
22+
y; % Y, m, UTM coors
23+
velocity; % d_Yb, body velocity
24+
first_heading; % heading, rad, world frame
25+
0]; % d_heading, rad/s
26+
27+
load('./waypoints.mat');
28+
desired_traj = processWaypoints(logs);
29+
% time = linspace(0, 240, size(trajectory,2));
30+
time = 0:dt:240;
31+
u = 0; % commanded steering angle
32+
steering = u; % steering angle
33+
trajectory = [X; lat_long(1); lat_long(2); steering];
34+
35+
for i = 1:size(time, 2)
36+
t = time(i);
37+
A = model(X, steering);
38+
X = A*X;
39+
steering = updateSteering(steering, u);
40+
41+
X(4) = clampAngle(X(4));
42+
X(5) = clampAngle(X(5));
43+
44+
if(mod(i, m) == 0)
45+
u = control(desired_traj, X);
46+
end
47+
48+
% trajectory = [trajectory, X];
49+
snapshot = summarize(X, utm_zone, steering);
50+
trajectory = [trajectory, snapshot];
51+
end
52+
53+
if save_data
54+
save('controller_v2.mat', 'trajectory');
55+
end
56+
end
57+
58+
function [desired] = processWaypoints(lat_long)
59+
[x, y, zone] = ll2utm(lat_long);
60+
desired = [x y];
61+
end
62+
63+
function snapshot = summarize(x, utm_zone, steeringAngle)
64+
[lat, lon] = utm2ll(x(1), x(2), utm_zone);
65+
snapshot = [x; x(1); x(2); steeringAngle];
66+
end
67+
68+
function a = clampAngle(a)
69+
while (a < -pi)
70+
a = a + 2*pi;
71+
end
72+
while (a > pi)
73+
a = a - 2*pi;
74+
end
75+
end
76+
77+
function b = updateSteering(b, u)
78+
global steering_vel
79+
global dt
80+
81+
if(b < u)
82+
b = b + steering_vel*dt;
83+
end
84+
if(b > u)
85+
b = b - steering_vel*dt;
86+
end
87+
end
88+
89+
function a = clampSteeringAngle(a)
90+
if(a < -deg2rad(10))
91+
a = -deg2rad(10);
92+
end
93+
if(a > deg2rad(10))
94+
a = deg2rad(10);
95+
end
96+
end
97+
98+
function [A] = model(x, steering)
99+
global wheel_base
100+
global dt
101+
102+
A = [1, 0, dt*cos(x(4)), 0, 0;
103+
0, 1, dt*sin(x(4)), 0, 0;
104+
0, 0, 1, 0, 0;
105+
0, 0, 0, 1, dt;
106+
0, 0, tan(steering)/wheel_base, 0, 0];
107+
end
108+
109+
function [u] = control(desired_traj, X)
110+
pos = X(1:2);
111+
b = repmat(pos, size(desired_traj, 1), 1);
112+
delta = 15*15;
113+
possible = find(sum((desired_traj-b).^2, 2) < delta);
114+
if isempty(possible)
115+
u = 0;
116+
else
117+
target = desired_traj(possible(end), :);
118+
deltaPath = target - pos;
119+
u = atan2(deltaPath(2), deltaPath(1))-X(4);
120+
end
121+
u = clampSteeringAngle(clampAngle(u));
122+
end
123+
124+
125+

‎offline/controller/controller_analysis.m‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
% graph results
2+
3+
clear;
4+
close all;
5+
6+
addpath('../localizer/latlonutm/Codes/matlab');
7+
addpath('../localizer/altmany-export_fig');
8+
9+
file = 'controller_v2.mat';
10+
load(file, 'trajectory');
11+
save_plot = false;
12+
show_maps = true;
13+
14+
load('./waypoints_course_v2.mat');
15+
[x, y, zone] = ll2utm(logs);
16+
desired = [x y];
17+
desired = desired(112:(end-50), :);
18+
19+
k = 1000;
20+
if show_maps
21+
wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r')
22+
end
23+
24+
heading = trajectory(4,1:k:end);
25+
figure();
26+
hold on;
27+
plot(trajectory(1,1:k:end), trajectory(2,1:k:end))
28+
quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading))
29+
plot(desired(:,1), desired(:,2), 'g')
30+
hold off;
31+
title(['Map ', file]);
32+
33+
headingd = rad2deg(heading);
34+
figure();
35+
hold on;
36+
plot(1:length(headingd), headingd)
37+
title(['Heading ', file]);
38+
39+
figure();
40+
p = k/20;
41+
plot(1:p:size(trajectory,2), trajectory(8,1:p:end))
42+
title('Control input');
43+
44+
% plot on google maps
45+
if show_maps
46+
xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)];
47+
fprintf(1, '%5.20f, %5.20f\n', xy);
48+
end

‎offline/controller/controller_pure.m‎

Lines changed: 137 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
function [trajectory] = controller_pure()
2+
% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
3+
% section 2.2
4+
5+
addpath('../localizer/latlonutm/Codes/matlab');
6+
global wheel_base
7+
global velocity
8+
global steering_vel
9+
global dt
10+
11+
save_data = true;
12+
wheel_base = 1.13;
13+
utm_zone = 17;
14+
first_heading = deg2rad(250);
15+
lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362];
16+
dt = 0.001; % 1000Hz
17+
m = 50; % 20Hz
18+
velocity = 8; % m/s, 17.9mph, forward velocity
19+
steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds
20+
% full range in 0.5s
21+
22+
[x, y, ~] = ll2utm(lat_long(1), lat_long(2));
23+
24+
X = [x; % X, m, UTM coors
25+
y; % Y, m, UTM coors
26+
velocity; % d_Yb, body velocity
27+
first_heading; % heading, rad, world frame
28+
0]; % d_heading, rad/s
29+
30+
load('./waypoints_tri.mat');
31+
desired_traj = processWaypoints(logs);
32+
% time = linspace(0, 240, size(trajectory,2));
33+
time = 0:dt:60; % 240;
34+
u = 0; % commanded steering angle
35+
steering = u; % steering angle
36+
trajectory = [X; lat_long(1); lat_long(2); steering];
37+
38+
for i = 1:size(time, 2)
39+
t = time(i);
40+
A = model(X, steering);
41+
X = A*X;
42+
steering = updateSteering(steering, u);
43+
44+
X(4) = clampAngle(X(4));
45+
X(5) = clampAngle(X(5));
46+
47+
if(mod(i, m) == 0)
48+
u = control(desired_traj, X);
49+
end
50+
51+
% trajectory = [trajectory, X];
52+
snapshot = summarize(X, utm_zone, steering);
53+
trajectory = [trajectory, snapshot];
54+
end
55+
56+
if save_data
57+
save('controller_tri_v1.mat', 'trajectory');
58+
end
59+
end
60+
61+
function [desired] = processWaypoints(lat_long)
62+
[x, y, zone] = ll2utm(lat_long);
63+
desired = [x y];
64+
end
65+
66+
function snapshot = summarize(x, utm_zone, steeringAngle)
67+
[lat, lon] = utm2ll(x(1), x(2), utm_zone);
68+
snapshot = [x; x(1); x(2); steeringAngle];
69+
end
70+
71+
function a = clampAngle(a)
72+
while (a < -pi)
73+
a = a + 2*pi;
74+
end
75+
while (a > pi)
76+
a = a - 2*pi;
77+
end
78+
end
79+
80+
function b = updateSteering(b, u)
81+
global steering_vel
82+
global dt
83+
84+
if(b < u)
85+
b = b + steering_vel*dt;
86+
end
87+
if(b > u)
88+
b = b - steering_vel*dt;
89+
end
90+
end
91+
92+
function a = clampSteeringAngle(a)
93+
if(a < -deg2rad(10))
94+
a = -deg2rad(10);
95+
end
96+
if(a > deg2rad(10))
97+
a = deg2rad(10);
98+
end
99+
end
100+
101+
function [A] = model(x, steering)
102+
global wheel_base
103+
global dt
104+
105+
A = [1, 0, dt*cos(x(4)), 0, 0;
106+
0, 1, dt*sin(x(4)), 0, 0;
107+
0, 0, 1, 0, 0;
108+
0, 0, 0, 1, dt;
109+
0, 0, tan(steering)/wheel_base, 0, 0];
110+
end
111+
112+
function [u] = control(desired_traj, X)
113+
global wheel_base
114+
115+
pos = X(1:2)';
116+
pos_b = repmat(pos, size(desired_traj, 1), 1);
117+
delta = 15;
118+
cutoff = 100;
119+
delta = delta * delta;
120+
121+
distances = sum((desired_traj - pos_b).^2, 2);
122+
[~, closest_idx] = min(distances);
123+
last_idx = min([length(distances), closest_idx + cutoff]);
124+
possible = find(distances(1:last_idx) < delta);
125+
if isempty(possible)
126+
u = 0;
127+
else
128+
target = desired_traj(possible(end), :);
129+
deltaPath = target - pos;
130+
131+
k = 0.8;
132+
a = atan2(deltaPath(2), deltaPath(1))-X(4);
133+
u = atan2(2*wheel_base*sin(a), k*X(3));
134+
end
135+
u = clampSteeringAngle(clampAngle(u));
136+
end
137+

0 commit comments

Comments
 (0)