calculating position using follower wheels

[flameout]

I've refactored my code to be much faster (using basic manual Euler integration instead of MATLAB's integral() function) and plotted the robot's position for problem 5. The plot is attached.

Here is the code. I would have done this in C++, but MATLAB lets me plot much more easily

Spoiler for code:

% Set up various useful functions that don't require integration to calculate
rotate_CW = @(t) 1.5*sin(t/2.5);
forward = @(t) 5*sin(t/2);
strafe_right = @(t) 4*sin(t/2.2);
xvel = @(t,angle) cos(angle) * forward(t) + cos(angle - pi/2) * strafe_right(t);
yvel = @(t,angle) sin(angle) * forward(t) + sin(angle - pi/2) * strafe_right(t);
spd  = @(t) sqrt(forward(t)^2 + strafe_right(t)^2);

% Set parameters and initialize variables (preallocating variables for performance)
duration = 30;
steps    = 30000; %dt = 1 millisecond
dt       = duration / steps;
angle    = zeros(1,steps+1); % Off by one -- t = 0 is also in each vector
xpos     = zeros(1,steps+1);
ypos     = zeros(1,steps+1);
distance = zeros(1,steps+1);

% Initialize angle to 15 degrees positive off the Y axis
angle(1) = 5*pi/12;

% Do the integration. Using Euler integration
for iter = 1:steps
	time             = duration * iter / steps;
	xpos(iter+1)     = xpos(iter) + dt * xvel(time, angle(iter));
	ypos(iter+1)     = ypos(iter) + dt * yvel(time, angle(iter));
	distance(iter+1) = distance(iter) + dt * spd(time);

	% No intuitive tricks with angle here -- just do stupid simple Euler integration.
	angle(iter+1)    = angle(iter) - dt * rotate_CW(time);
end