%Uppgift 5, som 4 fast med modifikation
%Fall 1: aR = aL = 3, wR = 1, wL = 5
clc; clear all; close all;
aR = [3 4 1 1.2];
aL = [3 2 3 0.4];
wR = [1 3 6 3];
b = 1;
wL = [5 2 1 1];
x0 = 0;
y0 = 1.5;
theta0 = 0;
s0 = [x0 y0 theta0];
options = odeset('RelTol',1e-6,'Abstol',1e-6, 'refine', 4);
tspan = [0 10]';

for i = 1:4

    [t,s] = ode45(@(t,s) fvel(t,s,b,aL(i),aR(i),wL(i),wR(i)),[0 10],s0,options);
    figure(i)
    plot3(s(:,1),s(:,2),t); grid on; %figure 4 är uppgift 6 b

end



function ds = fvel(t,s,b,aL,aR,wL,wR)
vR = aR*t + wR;
vL = aL*t + wL;
v = (vR + vL)/2;
dtheta = (vR - vL)/b;
theta = s(3);
ds = [v*cos(theta), v*sin(theta), dtheta]';

end