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


#-- Robot parameters

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



x=0;
y=0;
teta=0;


#-- Oscillator's parameters
A1=60 * pi/180;
A2=60 * pi/180;
T1=40;
T2=40;
DF=120*pi/180;
PHI0=0 * pi/180;

#-- Move the robot
for t=[0:T1*10]

  #-- Velocity in the local frame
  #-- Wheel's velocity (Rad/s):

  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];


  #-- 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);

endfor;



