#-- SWINGING THE WHEEL!!!
#-- By (Juan Gonzalez)^2


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

#-- User input parameters
ka = 0.5;
direction = 0 * pi/180;  #-- Direction of movement
total_distance = 400;  #-- Total distance moved to finish the simulation




#-- Oscillator's parameters
O= -direction*l/r;      #--- Offset

if (O>=(pi/2))
  O=pi/2;
endif

A=ka*(pi/2-abs(O));
disp("A="); disp(A*180/pi);
T=30;
DF=90  *pi/180;
PHI0=0 * pi/180;    #--- Initial phase



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

#-- Initialization
teta1 = O;
teta2 = A*sin(DF) + O;

distance = 0;
t=0;

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

  #--- Calculations in the local frame

    #-- Store the previous wheel's angles
    teta1_old = teta1;  
    teta2_old = teta2;

    #-- Calculate the new wheel's angle
    teta1 = A*sin(2*pi*t/T) + O;
    teta2 = A*sin(2*pi*t/T + DF) + O;
  
    #-- Calculate the actual angular velocity (Rad/s)
    w1 = teta1 - teta1_old;
    w2 = teta2 - teta2_old;

    #w1 = (2*pi*A1/T1)*cos(2*pi*t/T1 + PHI0);
    #w2 = (2*pi*A2/T2)*cos(2*pi*t/T2 + DF + PHI0);

    #-- Calculate the linear and angular velocites from the wheel angular speed
    v = r/2*(w1+w2);
    w = r/(2*l) * (w1-w2);
    vl = [0  v  w];

  #-- Calculations in the global frame


    #-- Rotation matrix (z)
    c = cos(teta);
    s = sin(teta);
    Rot = [c -s 0;
           s  c 0;
           0  0 1 ];

    #-- Calculate the velocity in the global frame reference
    vg = Rot*vl';

    #-- Calculate the robot coordinates acording the new velocity
    x=x+vg(1);
    y=y+vg(2);
    teta=teta+vg(3);

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

   pause(0);

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

endwhile;



