%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   xPC Mobile Robot Control Program
%   12/05/2005
%   Benjamin Stephens & Peng Yang
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


close all;
clc;
terminate=0;

%%% DEFINE THE SIGNAL/PARAMETER INDICES %%%
SIGNAL_X = tg.getsignalid('Integrate/Integrate X');
SIGNAL_Y = tg.getsignalid('Integrate/Integrate Y');
SIGNAL_THETA = tg.getsignalid('Integrate/Integrate Theta');
PARAM_V = tg.getparamid('v_ref', 'Value');
PARAM_W = tg.getparamid('w_ref', 'Value');;
PARAM_MOTOR_ENABLE = tg.getparamid('Manual Switch/Constant','Value'); %'1'==off
PARAM_KP1 = tg.getparamid('Motors/Motor Controller/Kp1','Gain');
PARAM_KP2 = tg.getparamid('Motors/Motor Controller/Kp2','Gain');
PARAM_KI1 = tg.getparamid('Motors/Motor Controller/Ki1','Gain');
PARAM_KI2 = tg.getparamid('Motors/Motor Controller/Ki2','Gain');

%%% SETUP MOBILE ROBOT PARAMETERS %%%
ref_v = 0;
ref_w = 0;
delta_v=2.5;
delta_w=0.25;

%%% SETUP THE TIMER FUNCTION %%%
tt = [] ;
delay_seconds = 0.2;
tt = timer ;
tt.timerfcn = 'uiresume' ;
tt.startdelay = delay_seconds ;            

%%% CREATE THE FIGURE TO HOLD THE ANIMATION AND CAPTURE KEYBOARD INPUT %%%
callstr = ['set(gcbf,''Userdata'',double(get(gcbf,''Currentcharacter''))) ; uiresume '] ;
f1=figure( ...
   'menubar','figure', ... 					% set to 'none' to turn off the tool bar
   'Position',[500, 400, 500, 500], ... 	% left edge, bottom, width, height
   'color', [1 1 1], ... 					% r g b
   'Name','Demo',...                        % name of the figure
   'keypressfcn',callstr, ...               % define keyboard presses as interrupts
   'windowstyle','modal',...                % keeps the window on top so that it can get keyboard input
   'userdata',-1);                          % define userdata matrix
axis([-100 100 -100 100]);
hold on;


%%% PRINT OUT A REFERNCE PATH %%%
% a1=axes( ...
%   'xlim', [-2 12], ...
%   'ylim', [-15 15], ...
%   'dataaspectratio',[1 1 1]);

% % draw the sin wave to follow:
% t = 0: 0.01: 10;
%                                             % no reference right now
% plot(t, 10* sin(t)); 


%%% DRAW A LITTLE TRIANGULAR ROBOT %%%
SIZE_SCALE=2;
pos = SIZE_SCALE * [-1 0; 1 0; 0 4];
tria = patch('vertices',pos,'faces',[1:length(pos)],'edgecolor','none', ...						% patch graphic object
        'facecolor','r','LineWidth',2);

 
%%% START THE PROGRAM ON THE MOBILE ROBOT %%%
+tg;
tg.setparam(PARAM_MOTOR_ENABLE,1);
tg.setparam(PARAM_V,0);
tg.setparam(PARAM_W,0);
tg.setparam(PARAM_KP1,2.0);
tg.setparam(PARAM_KP2,2.0);

%%% ENTER THE PROGRAM LOOP %%%
while(1)
        
    %%% GET POSITION FROM MOBILE ROBOT %%%
    x=tg.getsignal(SIGNAL_X);
    y=tg.getsignal(SIGNAL_Y);
    theta=tg.getsignal(SIGNAL_THETA);
    
    %%% CAPTURE KEYBOARD INPUT %%%
    try
        % Wait for keyboard interrupt or the timer to run out
        start(tt) ;    
        uiwait ;
        ch = get(f1,'Userdata') ;
    catch
        % Something went wrong, return and empty matrix.
        ch = [] ;
    end
    stop(tt);
    set(f1,'Userdata',-1);
    
    %%% INTERPERET KEYBOARD INPUT %%%
    switch(lower(ch))
        case 'w'
            ref_v = ref_v + delta_v;
        case 's'
            ref_v = ref_v - delta_v;
        case 'a'
            ref_w = ref_w + delta_w;
        case 'd'
            ref_w = ref_w - delta_w;
        case 'q'
            terminate=1;
        case '.'
            kp=tg.getparam(PARAM_KP1);
            tg.setparam(PARAM_KP1,kp+0.5);
            tg.setparam(PARAM_KP2,kp+0.5);
        case ','
            kp=tg.getparam(PARAM_KP1);
            tg.setparam(PARAM_KP1,kp-0.5);
            tg.setparam(PARAM_KP2,kp-0.5);
        case 'm'
            if (tg.getparam(PARAM_MOTOR_ENABLE)==0)
                tg.setparam(PARAM_MOTOR_ENABLE,1);
            else
                tg.setparam(PARAM_MOTOR_ENABLE,0);
            end
        otherwise
    end  
    
    %%% SEND VELOCITY COMMANDS TO MOBILE ROBOT %%%
    tg.setparam(PARAM_V,ref_v);
    tg.setparam(PARAM_W,ref_w);

    %%% UPDATE THE ANIMATION %%%
    pos1 = [x + SIZE_SCALE*1*sin(theta) y - SIZE_SCALE*1* cos(theta)];
    pos2 = [x - SIZE_SCALE*1*sin(theta) y + SIZE_SCALE*1* cos(theta)];
    pos3 = [x + SIZE_SCALE*4*cos(theta) y + SIZE_SCALE*4* sin(theta)];
    set(tria,'vertices',[pos1;pos2;pos3]);
    plot(x,y);
    
    drawnow;
    
    %pause(0.01);
    
    if (terminate==1)   break;  end

end
tg.setparam(PARAM_MOTOR_ENABLE,1);
set(f1,'windowstyle','normal');
delete(tt);
-tg;
