image=imread('e:\matlab7\work\ui.jpg');
mag=input('DEI GRID NAVIGATOR ROBOT ROVER greets you..!please specify the magnification factor');
sd=input('please enter the step distance of grid navigator');% distance moved in 1 step in millimeters =0.823 cms
sa=input('please enter the step angle of grid navigator'); % angle moved in 1 step in degrees = 1.8 degree default
[x0 y0 val]=impixel(image);
[len ns]= size(x0);
x0=x0-59;
y0=445-y0;
x=x0;
y=y0;


% % robot configuration

angle(1)=0;
sense(1)=0;
distance(1)=0;
for i=2:len
    distance(i)=sqrt(  (x(i)-x(i-1))^2 +((y(i)-y(i-1))^2)  );  % contains information about distances to be moved at each point
end
for i=1:len-1
        theta=angle(i);
        if (sense(i)==1)
            theta=-theta;
        end
        xorigin=x(i);
        yorigin=y(i);
            for j=i:len  %translation of axis to define the new path wrt current frame of reference
                x(j)=x(j)-xorigin;
                y(j)=y(j)-yorigin;
            end

            for j=i+1:len  %rotation of axis to define the new path wrt current frame of reference
               xorigin=x(j);
               x(j)=(x(j)*cosd(theta)) - (y(j)*sind(theta));
               y(j)=(xorigin*sind(theta)) + (y(j)*cosd(theta));
            end
            
        if (x(i+1)==0)
            alpha=90;
        else
            alpha=abs( atand(  y(i+1)/x(i+1)  ) );
        end

                    if ( ( y(i+1)>=0 ) && ( x(i+1)>=0) )  % 1= anticlockwise sense, 0= clockwise sense
                        angle(i+1)=alpha;
                       sense(i+1)=1;
                    end

                    if ( ( y(i+1)>=0 ) && ( x(i+1)<0) )
                        angle(i+1)=180-alpha;
                        sense(i+1)=1;
                    
                    end

                    if ( ( y(i+1)<0 ) && ( x(i+1)<0) )
                        angle(i+1)=180-alpha;
                        sense(i+1)=0;
                    
                    end

                    if ( ( y(i+1)<0 ) && ( x(i+1)>=0) )
                        angle(i+1)=alpha;
                        sense(i+1)=0;
                    end
    
end
plot(x0,y0,'--rs','LineWidth',2,...
                'MarkerEdgeColor','k',...
                'MarkerFaceColor','g',...
                'MarkerSize',10);
xlabel('x path co ordinates')
ylabel('y path co ordinates')
title('THE NAVIGATOR TRAJECTORY')
for i=1:len-1
    distance1(i)=distance(i+1);
    angle1(i)=angle(i+1);
    direction(i)=sense(i+1);
end

dsteps=round(0.5*(mag*(distance1/sd)));
asteps=round(angle1/sa);
dsteps
asteps
direction

s1='path_no= ';
sc=';';
bc='}';
comma=',';
path_no=int2str(len-1);

fid=fopen('F:\detd\navigator\navigator1.c','at+') %the path of the C file , which this program opens and appends. The C file
                                                   % is finally input into the microcontroller
count=fwrite(fid,s1);
count = fwrite(fid,path_no);
count = fwrite(fid,sc);

for i=1:len-1
    no=int2str(i-1);
    e=int2str(dsteps(i));
    string=strcat('distance[',no);
    string=strcat(string,'] = ');
    count=fwrite(fid,string);
    count = fwrite(fid,e);
    count = fwrite(fid,sc);
    
end

for i=1:len-1
    e=int2str(direction(i));
    no=int2str(i-1);
    string=strcat('sense[',no);
    string=strcat(string,'] = ');
    count=fwrite(fid,string);
    count = fwrite(fid,e);
    count = fwrite(fid,sc);
end

for i=1:len-1
    e=int2str(asteps(i));
    no=int2str(i-1);
    string=strcat('angle[',no);
    string=strcat(string,'] = ');
    count=fwrite(fid,string);
    count = fwrite(fid,e);
    count = fwrite(fid,sc);
end
count = fwrite(fid,bc);
status=fclose(fid);