% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 
% % % % % % % Using MATLAB to send commands to an RC Quadrotor % % % % % % 
% % % % % % % % % % % Abdel-Razzak MERHEB 16/6/2016 % % % % % % % % % % % %
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% Clear everything before start
clear all
close all
clc 

% Define and initialize the serial port
SerPIC = serial('COM12');   % (COM12 or COM11) Change this appropriately
set(SerPIC,'BaudRate', 9600, 'DataBits', 8, 'Parity', 'none','StopBits', 1, 'FlowControl', 'none');              
fopen(SerPIC);  % Open the serial port to the PIC

% Get the Keypress from the user and send commands via the serial port
while(1)
    disp('Use Arrows to move forward/backward and left/right, W for up and S for down, D for yaw + and A for yaw -, and press ENTER to quit')
    P = getkey() % Use getkey function to read the keypress
    if (P == 30)
        disp(' ')
        disp('Go FORWARD')
        disp(' ')
        fprintf(SerPIC, '%s', 'F'); % Go Forward (Up arrow)
    elseif (P == 31)
        disp(' ')
        disp('Go BACKWARD')
        disp(' ')
        fprintf(SerPIC, '%s', 'B'); % Go Backward (Down arrow)
    elseif (P == 29)
        disp(' ')
        disp('Go RIGHT')
        disp(' ')
        fprintf(SerPIC, '%s', 'L'); % Go Left (Left arrow)
    elseif (P == 28)
        disp(' ')
        disp('Go LEFT')
        disp(' ')
        fprintf(SerPIC, '%s', 'R'); % Go Right (Right arrow)
    elseif (P == 119)
        disp(' ')
        disp('Go UP')
        disp(' ')
        fprintf(SerPIC, '%s', 'U'); % Go Up (W)
    elseif (P == 115)
        disp(' ')
        disp('Go DOWN')
        disp(' ')
        fprintf(SerPIC, '%s', 'D'); % Go Down (S)   
        
    elseif (P == 100)
        disp(' ')
        disp('Turn CW')
        disp(' ')
        fprintf(SerPIC, '%s', 'C'); % Go Up (D)
    elseif (P == 97)
        disp(' ')
        disp('Turn CCW')
        disp(' ')
        fprintf(SerPIC, '%s', 'W'); % Go Down (A)        
%     elseif (P == 32)
%         disp(' ')
%         disp('STOP')
%         disp(' ')
%         fprintf(SerPIC, '%s', 'S'); % Stop (Space)
        
        
    elseif (P == 13)
        disp(' ')
        disp('Quit')
        disp(' ')
        fprintf(SerPIC, '%s', 'S'); % STOP
        break;
    else
        disp('Wrong button pressed')
    end
    
end

% Close the serial port when dones
fclose(SerPIC)                          
delete(SerPIC)				
clear SerPIC


%NOTE 1:
%if MATLAB ever gives a serial error, it will most likely say 'unable to open serial port' next time you
%run the program, so you have to restart MATLAB
