% generate commands for velocity and angle for each wheel
%
% robot coordinate frame (North, East, Down)
%    x = forward, xdot = forward velocity
%    y = right,   ydot = right velocity
%    z = down, positive rotation is clockwise from above
%    psidot is rotation rate around z
%
% velocity commands (xdot, ydot) are units of choice (ft/sec, in/sec, etc)
% rotation rate (psidot) needs to be in deg/sec
%
%    v1c = velocity command for swerve module 1
%    t1c = steering (theta) command for sterring module 1
%    v2c, t2c = module 2, etc.

function[v1c, t1c, ...
         v2c, t2c, ...
         v3c, t3c, ...
         v4c, t4c] = swerve_cmd(xdot, ydot, psidot);

% convert psidot to rad/sec
psidot = psidot*pi/180;

% wheel locations (x, y pairs, insert real dimensions here)
%   don't forget to match units to velocity cmd (ft & ft/sec, or in & in/sec)
%     4  1
%     3  2
% p1 = [ 12,  6];
% p2 = [-12,  6];
% p3 = [-12, -6];
% p4 = [ 12, -6];

cr = [14, 14];
p1 = [24.75, 24.75] - cr;
p2 = [ 3.25, 24.75] - cr;
p3 = [ 3.25,  3.25] - cr;
p4 = [24.75,  3.25] - cr;

% vector wheel velocities (v = w X r + vx + vy)

%        |  0,  0,  w |
%    v = | px, py,  0 | + i*xdot + j*ydot
%        |  i,  j,  k |
%
%    v = w*(px*j - py*i) + i*xdot + j*ydot
%
%    vx = xdot - w*py
%    vy = ydot + w*px

v1 = psidot*[-p1(2), p1(1)] + [xdot, ydot];
v2 = psidot*[-p2(2), p2(1)] + [xdot, ydot];
v3 = psidot*[-p3(2), p3(1)] + [xdot, ydot];
v4 = psidot*[-p4(2), p4(1)] + [xdot, ydot];

% wheel velocity command
v1c = sqrt(v1(1)^2 + v1(2)^2);
v2c = sqrt(v2(1)^2 + v2(2)^2);
v3c = sqrt(v3(1)^2 + v3(2)^2);
v4c = sqrt(v4(1)^2 + v4(2)^2);

% wheel angle command (convert to degrees for the radian-impared)
t1c = 180/pi*atan2(v1(2), v1(1));
t2c = 180/pi*atan2(v2(2), v2(1));
t3c = 180/pi*atan2(v3(2), v3(1));
t4c = 180/pi*atan2(v4(2), v4(1));