clear all
clc
%%
%Kartabmeßungen
b = 1.39; %Fahrzeugbreite [m]
l = 2.00; %Fahrzeuglaenge [m]
u_Rad = 0.91; %Radumfang [m]
d_Rad = u_Rad / (2* pi); %Raddurchmesser [m]
r_Rad = d_Rad/2; %Radradius [m]
ab_RM = 1.075; %Abstand der Radmittelpunkte der HA [m]
o_x_Kante_RM = 0.1625; %x Abstand von der seitlichen Kartkante zum Radmittelpunkt [m]
o_y_Kante_RM = 0.425; %y Abstand von der Heckkartkante zum Radmittelpunkt [m]




%%
%x und y Koordinaten der Kartkanten vom Ursprung der Achsmitte [0|0]
xKOS_HL = -b/2;
xKOS_HR = b/2;
xKOS_VR = b/2;
xKOS_VL = -b/2;
yKOS_HL = -o_y_Kante_RM;
yKOS_HR = -o_y_Kante_RM;
yKOS_VR = l - o_y_Kante_RM;
yKOS_VL = l - o_y_Kante_RM;

%Koordinaten der Kartkanten
HL = [xKOS_HL; yKOS_HL];
HR = [xKOS_HR; yKOS_HR];
VL = [xKOS_VL; yKOS_VL];
VR = [xKOS_VR; yKOS_VR];






%% 
%Winkelgeschwindigkeiten der einzeln angetriebenen Hinterräder
psi_HR_Punkt = 3; %Winkelgeschwindigkeit an der Radnabe HR
psi_HL_Punkt = 5; %Winkelgeschwindigkeit an der Radnabe HL





%%
%Simulink starten
sim('Simulation_Simulink')



%%
%plotten


%neue Kartkanten in Abhängigkeit des Gierwinkels + die Bewegung der Achsmitte
x = [(HL_gel.Data(:,1)) (HR_gel.Data(:,1)) (VR_gel.Data(:,1)) (VL_gel.Data(:,1)) (HL_gel.Data(:,1))];
y = [(HL_gel.Data(:,2)) (HR_gel.Data(:,2)) (VR_gel.Data(:,2)) (VL_gel.Data(:,2)) (HL_gel.Data(:,2))];




figure
grid on
axis equal
hold on

plot(x,y,'-b')

%%
%Probe Matrixrechnung; psi_HR_Punkt = 3 % psi_HL_Punkt = 5
A = [0 0.6238 0.7816 0 0; 0.5375 0.7816 -0.6238 -0.07242 0; -0.5375 0.7816 -0.6238 0 -0.07242];
B = [-0.1347; 0.2264; -0.1807; 3; 5];

M = A*B