Skhora

%Uppgift 4
clc; clear all;
close all;
format long;
theta0 = 0;
b = 1;
aR = 0;, aL = 0;, wL = 4;, wR = 2;, x0 = 0;
y0 = 1.5; %Gör om på rätt sätt, robotcircle med xbar = 0, vad blir då ybar? jo 1.5
s0 = [x0, y0, theta0]';
Tlap = pi;
t0 = 0;
tspan = [t0 Tlap];

B = (wR + wL)/2;
D = (wR - wL)/b;

xreal = [];
yreal = [];
thetareal = [];


%ode45
options = odeset('RelTol',1e-8,'Abstol',1e-8, 'refine', 1);
[t,s] = ode45(@(t,s) fvel(t,s,b,aL,aR,wL,wR),tspan,s0,options);
thetapprox = s(:,3);

for j = 1:length(t)
    thetareal(j) = D*t(j) + theta0;
end
thetareal = thetareal';
for i = 1:length(t)
   xreal(i) = x0 + (B/D)*sin(thetareal(i));
   yreal(i) = y0 - ((B/D)*(cos(thetareal(i)) - 1));
end
xreal = xreal';
yreal = yreal';

xrelfel = abs(xreal-s(:,1));
yrelfel = abs(yreal-s(:,2));

plot(t,xrelfel); hold on; grid on;
plot(t,yrelfel);
legend('Felet i X','Felet i Y');


function [xbar,ybar,R,Tlap] = robotcircle(b,wL,wR,x0,y0,theta0)
R = abs((b*wR + b*wL)/(2*wR-2*wL));
xbar = x0 - (b*wR + b*wL)/(2*wR - 2*wL)*sin(theta0);
ybar = y0 + (b*wR + b*wL)/(2*wR - 2*wL)*cos(theta0);
Tlap = 4*pi*R/(wR + wL);
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