Exercise 12, Task 6

The function below creates a file that contains instructions for the robot depending on the resulted list of cells.



function SMRfile(q)
fid = fopen('SMRcommand.txt','w');
for i=1:(size(q,1)-1)
    x= 0.05*(q(i+1,1)-q(i,1));
    y= 0.05*(q(i+1,2)-q(i,2));
    
    if(y==0 && x>0)
        th=0;
    elseif(y==0 && x<0)
        th=180;
    elseif(x==0 && y>0)
        th=90;
    elseif(x==0 && y<0)
        th=-90;
    elseif(x>0 && y>0)
        th=45;
    elseif(x>0 && y<0)
        th=-45;
    elseif(y>0)
        th=135;
    else
        th=-135;
    end
    
    fprintf(fid,'drive %f %f %f:($targetdist<0)\n',x,y,th);
end
fprintf(fid,'stop');
fclose(fid)
end