%================================= lynx6 =================================
%
%
%
%
%================================= lynx6 =================================
%
%
%  Name:	    lynx6.m
%
%  Author:	    Patricio A. Vela, pvela@gatech.edu
%
%  Created:	    2007/08/09
%  Modified:	2014/03/26
%
%================================= lynx6 =================================
classdef lynx6 < handle


properties
  alpha;

  alphaIds    = [   0    1    2    3    4    5];		
  				% Servo ID numbers on the SSC-32 control board.
  
  alphaHome   = [   0;   0;   0;   0;   0; 1.0];		
  				% Home position as a joint configuration.
  %alphaSleep  = [   0,  75, -120, -75,   0, 1.0];	% Folded up.
  alphaSleep  = [   0;  35;   70;  70;   0;  1.0];	% Leaning on block.
  				% Sleep position as a joint configuration.
  				%   This is what you put it to before powering 
        				%   down.
  
  alphaOrient = [ -1,   1,   1,  -1,   1,  -1];		
  				% To switch orientation in case servo rotates 
        				%   in the wrong direction 
        				%   (e.g. need right-hand rule).
  
  alphaLims = [ -90, -90, -90, -90, -80,  0.00;		
                  0,   0,   0,   0,   0,  0.75;
       	         90,  90,  90,  90,  80, 1.125];
  				% Limits of the joints, either angular values 
      			%   or linear values as in the gripper.
  				%   The middle value is some measured
  				%   intermediate location.
  musecLims = [ 500  500  500  500  500  500;			
               1500 1500 1500 1500 1500 1500;			
               2500 2500 2500 2500 2500 2500];
  				% How these limits translate to microseconds 
        				%   for the servo commands.
  
  linklen;
  serialid;
end

%)
%
%============================ Member Functions ===========================
%
%(

methods

  %------------------------------- lynx6 -------------------------------
  %
  %  Constructor
  %
  function this = lynx6(setup)

  mm2in   = 1/25.4;
  linklen = [110.6 120 120 130 20]*mm2in;			
  				% Measured link lengths of the manipulator in
  				%   millimeters but converted to inches.

  % Serial port setup defaults:
  serport.com  = 'COM5';		% COM port is COM1.
  serport.baud = 9600;		% Baud rate is 9600 bps.
  serport.terminator = ';';	% Message terminator is Carriage Return.

  % Parse the setup argument now.
  if (nargin > 0)
    fnames = fieldnames(setup);
    for ii = 1:length(fnames)
      fnames{ii}
      switch fnames{ii}
        case {'linklen','alphaHome','alphaOrient'},
          eval(['this.' fnames{ii} ' = getfield(setup, fnames{ii});']);

          case 'musecLims',
          disp('Here 2');
          if (size(setup.musecLims,2) == 6)
  	        if (size(setup.musecLims, 1) == 2)
  	          this.musecLims(:,1) = setup.musecLims(:,1);
  	          this.musecLims(:,3) = setup.musecLims(:,2);
  	          this.musecLims(:,2) = mean(setup.musecLims,1)
  	        elseif (size(setup.musecLims, 1) == 3)
  	          this.musecLims = setup.musecLims;
  	        end
          end

        case 'alphaLims'
          disp('Here 3');
          if (size(setup.alphaLims,2) == 6)
            if (size(setup.alphaLims,1) == 2)
  	          this.alphaLims(:,1) = setup.alphaLims(:,1);
  	          this.alphaLims(:,3) = setup.alphaLims(:,2);
  	          this.alphaLims(:,2) = mean(setup.alphaLims,1)
  	        elseif (size(setup.alphaLims, 1) == 3)
  	          this.alphaLims = setup.alphaLims;
  	        end
          end

        case 'com','baud','terminator',
          eval(['serport.' fnames{ii} ' = getfield(setup, fnames{ii});']);
      end
    end
  end
  this.musecLims
  this.alphaLims
  this.alphaOrient
  % Open up the serial port.
  if (ismac)
    this.serialid = serial(serport.com,'BaudRate',serport.baud, ...
                                       'Terminator',serport.terminator);
    fopen(this.serialid);

  elseif (ispc)
    this.serialid = serial(serport.com,'BaudRate',serport.baud, ...
                                       'Terminator',serport.terminator)
    fopen(this.serialid)
    % Note that in Windows, if the program crashes then the com port is
    % still open.  You will not be able to reopen the port and you won't
    % be able to access the serialid you had before.  The only option I
    % know of is to restart Matlab.  Or you could try to do an 'fclose
    % all' and see if that works.  I haven't tested it out.
    %
    %   Read documentation: 'help fclose'
  
  elseif isunix
    disp('Trying ttyACM0');
    device = '/dev/ttyACM0'
    this.serialid = fopen(device,'w+');
    if (this.serialid == -1)  % ACM0 didn't work
      disp('Trying ttyACM1');
      device(end) = '1';  % try ACM1
      this.serialid = fopen(device,'w+');
    end
    if (this.serialid == -1)
      disp('Could not connect to device!');
    end
  end

  this.serialid
  fprintf(this.serialid, 'E');
  
  end

  %--------------------------- setJointLimits --------------------------
  %
  function setJointLimits(this, newAlpha, newMusec, newOrient)

  OK = all(size(this.alphaLims) == size(newAlpha))
  OK = all(size(this.musecLims) == size(newMusec))
  OK = all(size(this.alphaOrient) == size(newOrient))

  this.alphaLims = newAlpha;
  this.musecLims = newMusec;
  this.alphaOrient = newOrient;

  end

  %---------------------------- alpha2musec ----------------------------
  %
  %
  function ticks = alpha2musec(this, alpha)

  if ( ((size(alpha,1) ~= 5) && (size(alpha,1) ~= 6))  ...
                || (size(alpha,2) ~= 1) )
    disp('Incorrect dimensions, ignoring command');
    disp('    [expect: 5x1 or 6x1 column vector].');
    ticks = [];
    return;
  end

  inds = [1:size(alpha,1)];
  alpha = transpose(min(this.alphaLims(3,inds),max(this.alphaLims(1,inds), ...
                                                                    alpha')));
  ticks = zeros(size(alpha));
  for ii = 1:size(alpha,1)
    if (this.alphaOrient(ii) == 1)
      ticks(ii) = interp1(this.alphaLims(:,ii), this.musecLims(:,ii), ...
                          alpha(ii));
    else
      ticks(ii) = interp1(this.alphaLims(end:-1:1,ii), this.musecLims(:,ii), ...
                          alpha(ii));
    end
  end
  ticks = round(ticks);  % round avoida sending scientific notation strings

  end
  
  %---------------------------- musec2alpha ----------------------------
  %
  %
  function alpha = musec2alpha(this, musec)

  if ( ((size(musec,1) ~= 5) && (size(musec,1) ~= 6))  ...
                || (size(musec,2) ~= 1) )
    disp('Incorrect dimensions, ignoring command');
    disp('    [expect: 5x1 or 6x1 column vector].');
    return;
  end

  musec = transpose(min(this.musecLims(3,:),max(this.musecLims(1,:), musec')));
  alpha = zeros(size(musec));
  for ii = 1:size(musec,1)
    if (this.alphaOrient(ii) == 1)
      alpha(ii) = interp1(this.musecLims(:,ii), this.alphaLims(:,ii), ...
                          musec(ii));
    else
      alpha(ii) = interp1(this.musecLims(end:-1:1,ii), this.alphaLims(:,ii), ...
                          musec(ii));
    end
  end

  end
  
  %----------------------------- gotoSleep -----------------------------
  %
  %  The first and second to last member function to call.  This puts
  %  the manipulator into a rest position for shutdown.  At startup, it
  %  returns the manipulator to the rest position.  This is to prevent
  %  huge jerky movements as the motor goes to the first commanded
  %  position from wherever it is located at.
  %
  function gotoSleep(this, time)
  
  if (nargin == 1)
    time = 2;
  end
  
  this.setArm(this.alphaSleep, time);
  
  end

  %------------------------------ gotoHome -----------------------------
  %
  %  This is the default position after achieving the sleep position.
  %  The home position is typically where it will go when powered up,
  %  but not being used.
  %
  function gotoHome(this, time)
  
  if (nargin == 1)
    time = 2;
  end
  
  this.setArm(this.alphaHome, time);
  
  end
  
  
  %----------------------------- setServos -----------------------------
  %
  %  Send raw motor commands to the servomotors.
  %
  function setServos(this, pwmsigs, time, defaultLims)
  
  if ((nargin < 3) || isempty(time))
    time = 2;
  end
  
  if (nargin < 4)
    defaultLims = true;
  end
    
  if ( (size(pwmsigs,1) ~= 6) || (size(pwmsigs,2) ~= 1) )
    disp('Incorrect dimensions, ignoring command [expect: 6x1 column vector].');
    return;
  end
    
  if (defaultLims)
    ticks = min(this.musecLims(3,:),max(this.musecLims(1,:), pwmsigs'));
  else
    ticks = min(2500, max(500, pwmsigs'));
  end
  
  cmdstr = sprintf('#%d P%d ',[this.alphaIds ; ticks]);		
  cmdstr = [cmdstr sprintf('T%d', round(time*1000))]
  fprintf(this.serialid, cmdstr);
 
  end
  
  %------------------------------- setArm ------------------------------
  %
  %  Send the arm to some position, with an optional time duration of
  %  movement.  If the gripper width is not passed, then defaults to the
  %  home position gripper width.
  %
  function setArm(this, alpha, time)

  if (nargin == 2)
    time = 4;
  end

  ticks = this.alpha2musec(alpha);
  ticks = round(ticks);  % round avoida sending scientific notation strings
  
  cmdstr = sprintf('P%d ', [ticks]);
  cmdstr = [cmdstr sprintf('T%d', round(time*1000))];
  fprintf([ cmdstr '\n']);
  fprintf(this.serialid, cmdstr);
  
  end

  %----------------------------- displayArm ----------------------------
  %
  %  Display the manipulator.  Joint angles should be in degrees since
  %  the are converted.  (Or remove the line giving the conversion and
  %  let it accept radians.  Up to you.)
  %
  %(
  function displayArm(this, alphadisp)

  if (nargin == 1)
    alphadisp = this.alpha;
  end

  vparms.home = 'straight-up';
  alphadisp = diag([(pi/180)*ones([1 5]), 1])*alphadisp;	% Convert to rads.
  lynx6_display(alphadisp, linklen, vparms);

  end


  %)
  %----------------------------- forwardkin ----------------------------
  %
  %  Compute the forward kinematics of the manipulators.  If the
  %  manipulator was calibrated and the measurements taken so that it is
  %  possible to select either the middle of the grip region, or just
  %  the tip of the fingers, then totip would toggle between forward
  %  kinematics for one and the other.
  %
  %  Make sure that the SE3 class is in your path.
  %
  function g = forwardkin(this, alpha, totip)
  
  if (nargin == 2)
    totip = false;
  end
  
  g = SE3();
  % DOES NOTHING, STUDENT TO CODE.
  
  end
  
  %----------------------------- inversekin ----------------------------
  %
  %  Compute the inverse kinematics of the manipulator.  Either to get
  %  the finger tips to the desired configuration or the middle of the
  %  grip region (second is default).  Also, solPick is an optional
  %  argument specifying which of the various solutions to choose from.
  %  At minimum, it should specify the upper or the lower solution.
  %  A second option would be to specify if it will be a turn around
  %  and reach backwards solution or a reach forwards.
  %
  %  Gripper width is ignored in these inverse kinematics, so only
  %  a 5x1 column vector is returned.
  %
  function alpha = inversekin(this, gdes, totip, solPick)
  
  if ( (nargin <= 2) || isempty(totip) )
    totip = false;
  end
  
  if (nargin < 4)
    solPick = [];       % Up to you to decide how to use.
  end
  
  alpha = zeros(5,1);
  % DOES NOTHING, STUDENT TO CODE;
  
  end
  
  %---------------------------- posJacobian ----------------------------
  %
  %  Computes the manipulator Jacobian for the position part only.
  %  The frame of reference is the world coordinate system (it is
  %  not done in body coordinates).
  %(
  function pJac = posJacobian(this, alpha)

  % Construction the Jacobian matrix.
  pJac = [];

  end

  %)
  %----------------------- genPositionTrajectory -----------------------
  %
  %  Given a desired position trajectory, generate the joint angles
  %  that will follow it.  Also assume that the initial joint angles
  %  are availalbe as an argument and that they map to the initial
  %  value of the position trajectory.
  %
  %  ptraj tspan should be valid over the duration of the actual
  %  tspan.  There are no checks here for that case, just assumed
  %  to be the case.
  %
  function alphaTraj = genPositionTrajectory(this, ptraj, tspan)

  % Resolved rate code here.  Use posODE as the ode function.

  [alphaTraj.time, alphaTraj.alpha] = odeCallHere;

    %------------------------- posODE ------------------------
    %
    %  this function can access the variables in 
    %  genPositionTrajectory, so it is all good.
    %  Access ptraj properly here.
    %
    function alphaDot = posODE(t, alpha)

    alphaDot = zeros(5,1);

    % Fill in the ODE here.

    % Make sure to convert angular rates from radians per second
    % to degrees per second.  Use diag function to help you.
    alphaDot = conversionHere;

    end

  end

  %------------------------------ Jacobian -----------------------------
  %
  %  Computes the manipulator Jacobian for the lynx6 manipulator.
  %  In this case, it should be the body Jacobian.
  %
  %(
  function mJacBody = Jacobian(this, alpha)

  % Construction the Jacobian matrix.
  mJacBody = [];

  end

  %)
  %--------------------------- genTrajectory ---------------------------
  %
  %  Given a desired position trajectory, generate the joint angles
  %  that will follow it.  Also assume that the initial joint angles
  %  are availalbe as an argument and that they map to the initial
  %  value of the position trajectory.
  %
  %  ptraj tspan should be valid over the duration of the actual
  %  tspan.  There are no checks here for that case, just assumed
  %  to be the case.
  %
  function alphaTraj = genTrajectory(this, ptraj, tspan)

  % Resolved rate code here.  Use groupODE as the ode function.

  [alphaTraj.time, alphaTraj.alpha] = odeCallHere;

    %------------------------ groupODE -----------------------
    %
    %  this function can access the variables in 
    %  genPositionTrajectory, so it is all good.
    %  Access ptraj properly here.
    %
    function alphaDot = groupODE(t, alpha)

    alphaDot = zeros(5,1);

    % Fill in the ODE here.

    % Make sure to convert angular rates from radians per second
    % to degrees per second.  Use diag function to help you.
    alphaDot = conversionHere;

    end

  end

  %--------------------- followJointTrajectory --------------------
  %
  %  Given a desired joint trajectory, command the joint angles
  %  that will follow it for the desired time span passed as a
  %  second argument.
  %
  %  jtraj tspan should be valid over the duration of the desired
  %  tspan.  There are no checks here for that case, just assumed
  %  to be the case.
  %
  function followJointTrajectory(this, jtraj, tspan)

  % Code to command trajectory to follow here.
  %
  % 1. Generate linear spacing in time across tspan.
  % 2. Interpolate the alpha values for these times (use interp1).
  % 3. Compute the time differences between times.
  % 4. Using for loop from 2nd element to last element (1st is initial
  %     condition which we are at, so can ignore it), ...
  %    a] send manipulator to new joint witth time duration slightly
  %       more than delta t (say by 5-10%).
  %    b] wait for actual delta t.
  %    c] go to the next joint angle.
  %

  end

  %------------------------------ shutdown -----------------------------
  %
  %  Send to sleep then close the serial port.
  %
  function shutdown(this)
  
  this.gotoSleep();
  fclose(this.serialid);
  
  end

end

%
%
%============================ Helper Functions ===========================
%
%

methods(Static)

  %-------------------------- closeSerialPort --------------------------
  %
  %  Find an open port using instrfind and then close it.  If the serial
  %  port is not closed for some funny reason, then call this helper
  %  method, as per:
  %
  %  lynx6.closeSerialPorts();
  %
  %  Should work.  Not tested.
  %
  function closeSerialPorts

  ports = instrfind();
 
  for ii=1:length(ports)
    if ( strcmp(ports(ii).Status,'open') )
      fclose(ports(ii));
    end
  end

  end

end


end
%
%================================= lynx6 =================================