%drawbot.m - plots the robot & its wiskers on the course
%  written by: Shawn Lankton
%  for: ECE8843 (sort-of) fun (mostly)
%
%  This draws the robot so that it appears to scamper about on the course.
%  This makes it the simulation fun to watch and addictive to improve.
%  Some stuff is commented out such as the 10 pixel 'DANGER ZONE.'  Also,
%  this function should be modified if more rangefinders are added, you
%  use a robot not modeled as a circle, etc.
%
%  Inputs:
%    posn - [yposn, xposn, theta]
%    rad - radius of the robot's body
%    course - obstacle matrix (0 = obstacle ~0 = clear)
%
%  Outputs:
%    none
%

function drawbot(posn, rad, course)

hold off;
%show the course
imagesc(course), axis image off; colormap gray;

hold on;

%draw a little circle for the robot
angs = 0:pi/10:2*pi;
y = posn(1) + rad*sin(angs);
x = posn(2) + rad*cos(angs);
plot(y,x);

%draw 10cm 'DANGER ZONE'
% y = posn(1) + (rad+10)*sin(angs);
% x = posn(2) + (rad+10)*cos(angs);
% plot(y,x,'r:');

%draw little wiskers representing 10cm rangefinders
drawang([posn(1), posn(2), posn(3) + pi/8],10+rad);
drawang([posn(1), posn(2), posn(3) - pi/8],10+rad);