% Define parameters

B = 20; % Distance between the wheel centers (in cm)

R1 = 5; % Radius of the first wheel (in cm)
R2 = 6; % Radius of the second wheel (in cm)

% Define the angular rates of the two wheels as functions of time

t = 0:0.01:10; % Time from 0 to 10 seconds
W1 = 5 * sin(3 * t);
W2 = 4 * sin(3 * t + 1);

% Initialize arrays to store the positions and ICC

robot_x = zeros(size(t));
robot_y = zeros(size(t));
icc_x = zeros(size(t));
icc_y = zeros(size(t));

% Simulate the robot's motion

for i = 1:length(t)
% Calculate the linear and angular velocities of the robot
V = (R1 * W1(i) + R2 * W2(i)) / 2;
angular_velocity = (R2 * W2(i) - R1 * W1(i)) / B;

% Calculate the change in position of the robot

delta_x = V * cos(angular_velocity * t(i));
delta_y = V * sin(angular_velocity * t(i));

% Update the robot's position

if i == 1
robot_x(i) = delta_x;
robot_y(i) = delta_y;
else
robot_x(i) = robot_x(i - 1) + delta_x;
robot_y(i) = robot_y(i - 1) + delta_y;
end

% Calculate the ICC (Instantaneous Center of Curvature)

if angular_velocity == 0
% Robot moves in a straight line, ICC at infinity
icc_x(i) = NaN;
icc_y(i) = NaN;
else
icc_radius = V / angular_velocity;
icc_x(i) = robot_x(i) - icc_radius * sin(angular_velocity * t(i));
icc_y(i) = robot_y(i) + icc_radius * cos(angular_velocity * t(i));
end
end

% Plot the robot's path and ICC

figure;
plot(robot_x, robot_y, 'b', 'LineWidth', 2);
hold on;
plot(icc_x, icc_y, 'r--', 'LineWidth', 1.5);
xlabel('X (cm)');
ylabel('Y (cm)');
title('Robot Path and Instantaneous Center of Curvature (ICC)');
legend('Robot Path', 'ICC');
grid on;