Exercise 3, Problem 10

The function for rotation has the following form:


function move = turn(angle,speed)
   global robotpar ts pose;

   d = robotpar(1);
   size = round(abs(angle)*d/(2*speed*ts));   %size of the array
   array = zeros(size,3);                                 %initializing the array

   M = [            0;
                        0;
        (angle/abs(angle))*2*speed/d];
  
   for i=1:size
       th = pose(3);
  
       Rinv = [cos(th) -sin(th)  0;
                    sin(th)  cos(th)  0;
                       0          0       1];
      
       array(i,:) = pose' + Rinv*M*ts;
       pose = array(i,:);
   end  
   move = array;
end

As in the previous problem, both positive and negative values for the angles can be given.