% nunchuck_operation.m
% Aaron Garcia
% 3/8/2020
% Connects to a nunchuk device at a specified serial port object and
% changes the x and y coordinates of the system on based upon inputs from
% the nunchuk

function nunchuck_operation(x_cur, y_cur, serialportobj)
%% Constants
scalenum = 0.1;
th1m=csvread('Theta1.csv');
th2m=csvread('Theta2.csv');
r1  = 5.55; %% lower arm length
r2  = 7; %% upper arm length

%% variables
zpress = 0;
xpress = 0;
ypress = 0;
x_inc = 0;
y_inc = 0;
data = 0;

%% open serial communication with arduino and wait for initialization
rxArduino = serialport('COM9', 9600);
pause(1);


%% Syncronize data with correct variables
while(data ~= 255)
    data = read(rxArduino, 1, "uint8");
end
disp(x_cur);
disp(y_cur);

%% Run until z button is pressed
while zpress==0
    xpress = read(rxArduino,1,"uint8");
    xinc = read(rxArduino,1,"uint8");
    ypress = read(rxArduino,1,"uint8");
    yinc = read(rxArduino,1,"uint8");
    data = read(rxArduino,1,"uint8");
    if xpress == 1 || ypress == 1 % if either of the x or y coordinates need to change
        if yinc == 0 % adjust increment values based on what was received (a zero corresponds to negative)
            yinc = -1;
        end
        if xinc == 0
            xinc = -1;
        end
        x_cur = x_cur + scalenum*(xpress*xinc); % change x and y values by the specified adjustment
        y_cur = y_cur + scalenum*(ypress*yinc);
        [th1,th2] = gettheta(x_cur, y_cur, th1m, th2m); % retreives new values for th1 and th2 from the precalculated look up table
        sendtofpga(serialportobj, x_cur, y_cur, 0, 0); % sends new valeus of x, y, th1, and th2 to the fpga
        
        fprintf("x: %f y: %f \nth1: %f th2: %f \n", x_cur,y_cur,th1, th2);
        node_x1 = r1*cosd(th1);             %%Lower arm x coordinate
        node_y1 = r1*sind(th1);             %%Lower arm y coordinate
        node_x2 = node_x1 + r2*cosd(th1+th2);   %%Upper arm x coordinate
        node_y2 = node_y1 + r2*sind(th1+th2);   %%Upper arm y coordinate 
        plot([0,node_x1],[0,node_y1],'b', 'linewidth',2);    %%Plot lower arm
        hold on
        plot([node_x1,node_x2],[node_y1,node_y2],'b','linewidth',2);
        grid on
        hold off
        xlabel('X-axis (in)')
        ylabel('Y-axis (in)')
        title('SCARA Robotic Arm Simulation')
        axis([-10 10 -10 10]) 
    end
    
    if data ~= 255
        zpress = 1;
    end
end

end