
#-- Robot parameters
r=30;    #-- Wheels radius (mm)
l=85/2;  #-- Distance between the com and the wheel (mm)

#-- Oscillator's parameters
A= 90   * pi/180;
O= 0    * pi/180;
T= 30;
PD= 90  * pi/180;
PHI0= 0  * pi/180;


#-- Robot's initial state
x=0;
y=0;
#-- Calculate the initial orientation
teta = r/(2*l)*(A*sin(PHI0)+O - A*sin(PD + PHI0) + O) + pi/2;

#-- Initialization
distance = 0;
t=0;
total_distance=400;

#-- Move the robot
while (distance<total_distance)

  #--- Calculations in the local frame

    #-- Wheels angle calculation
    phi1 = A*sin(2*pi*t/T + PHI0) + O;
    phi2 = A*sin(2*pi*t/T + PD+PHI0) + O;

    #-- Wheels angular velocity calculation
    d_phi1 = (2*pi*A/T)*cos(2*pi*t/T + PHI0);
    d_phi2 = (2*pi*A/T)*cos(2*pi*t/T + PD + PHI0);

  #-- Calculations in the global frame
  
    #-- Calculate the robot differential pose in the global frame
    dx = r/2 * (d_phi1 + d_phi2)*cos(teta);
    dy = r/2 * (d_phi1 + d_phi2)*sin(teta);
    d_teta = r/(2*l) * (d_phi1 - d_phi2);

    #-- Calculate the robot pose (integration)
    x = x + dx;
    y = y + dy;
    teta = teta + d_teta;
    

  #-- Draw the robot
   robot2(x,y,teta);

   pause(0);

   t=t+1;
   distance = sqrt(x^2 + y^2);

endwhile;



