PID controller for mouse game in matlab

Question

The game idea is to move a ball to a random position. It s based on levels,

LVL 0 - The ball postion is based on the mouse postion.

LVL 1 - The ball speed is based on the mouse position relative to the center.

LVL 2 - The ball acceleration is based on the mouse position relative to the center.

I decided to implement a PID controller for this.

I m having some trouble on the lvl 2. I cannot manage to move the ball to the required position.

This is the game GUI that generates the game. It s a lot of code I know, basically generates the game and write to a .mat file the ball postion. The script retreives the info from the .mat file to make the PID controller.

function varargout = MousePositioningGeneralizat(varargin)


% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @MousePositioningGeneralizat_OpeningFcn, ...
                   'gui_OutputFcn',  @MousePositioningGeneralizat_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before MousePositioningGeneralizat is made visible.

function createWriteLock 
    fclose(fopen('writelock', 'a'));

function deleteWriteLock
    delete('writelock');

function cw = canWrite
    fileExists = exist('readlock', 'file');
    if fileExists == 0
        cw = true;
    else
        cw = false;
    end

function MousePositioningGeneralizat_OpeningFcn(hObject, eventdata, handles, varargin)

% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to MousePositioningGeneralizat (see VARARGIN)

% Choose default command line output for MousePositioningGeneralizat
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% Init screen size
global xmax;
global ymax;
global stopflag;

screensize = get(0, 'ScreenSize')
xmax = screensize(3);
ymax = screensize(4);
stopflag = true;

level = get(handles.LevelSlider,'Value');
set(handles.Level, 'String', num2str(level-1));

set(handles.StartButton,'Visible','on');
set(handles.StopButton ,'Visible','off');

axes(handles.DisplayAxes);
plot(3 * xmax/4, ymax/2, 'ko', 'MarkerSize', 20)
text(15, ymax - 20, 'ECRAN', 'FontName', 'Times New Roman', 'FontSize', 14, 'Color', [0.5, 0.5, 0.5]);
hold on
plot(xmax/4, ymax/2, 'ko', 'MarkerFaceColor', 'm', 'MarkerSize', 16)
hold off
axis([1, xmax, 1, ymax]);

% UIWAIT makes MousePositioningGeneralizat wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.

function varargout = MousePositioningGeneralizat_OutputFcn(hObject, eventdata, handles)

% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;


% --- Executes on button press in StartButton.
function StartButton_Callback(hObject, eventdata, handles)
% hObject    handle to StartButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global xmax;
global ymax;
global stopflag;
global xfin;
global yfin;
global Level;
global vectorx;
global vectory;

set(handles.StartButton, 'Visible', 'off');
set(handles.StopButton , 'Visible', 'on');
stopflag = false;

% params
coeff = [1, 1000, 100, 10, 1, 1, 1, 1, 1, 1, 1];
% initial position
PL = get(0, 'PointerLocation');
vectorx = [PL(1), zeros(1, 10)];
vectory = [PL(2), zeros(1, 10)];
% final position
%xfin = xmax*unifrnd(0.3, 0.7);
xfin = 1500
disp(xfin)
%yfin = ymax*unifrnd(0.3, 0.7);
yfin = 750
disp(yfin)


historyx = PL(1);
historyy = PL(2);

TimeLim = 2.5; %s
thresh = 5; % pix
Ts = 0.1;
counter = 0;
ttime = 0;
Level = get(handles.LevelSlider, 'Value');
axes(handles.DisplayAxes);

cmpls = 0;

handles.myVar = xmax;
guidata(hObject, handles);


save('sharedData.mat', 'xmax', 'ymax', "stopflag", "xfin", "yfin", "Level");

while(counter < TimeLim && stopflag == false) % keep the dot there for 2.5 second
    PL = get(0, 'PointerLocation');
    
    if(Level == 1)
            x = PL(1);
            y = PL(2);
    else
        vectorx(Level) = (PL(1) - xmax/2) * coeff(Level) / (xmax/2);
        vectory(Level) = (PL(2) - ymax/2) * coeff(Level) / (ymax/2);    
        for k = Level - 1:-1:1
            disp("Here")
            vectorx(k) = vectorx(k) + Ts*vectorx(k+1);
            vectory(k) = vectory(k) + Ts*vectory(k+1);
        end
        disp("OUT")
        x = vectorx(1); % ball speed / ball position;
        y = vectory(1);
    end
  
    createWriteLock();
    
    if canWrite()
        save('sharedData.mat', 'xmax', 'ymax', "stopflag", "xfin", "yfin", "Level", "x", "y", 'cmpls', 'ttime');
        
        deleteWriteLock();
        
        pause(0.1);
    else
        deleteWriteLock();
        pause(0.1); 
    end

    x = max(11, x); x = min(xmax-10, x);
    %% 
    y = max(11, y); y = min(ymax-10, y);
    
    if(x ~= vectorx(1))
        vectorx = zeros(size(vectorx));
        vectorx(1) = x;
    end
    if(y ~= vectory(1))
        vectory = zeros(size(vectory));
        vectory(1) = y;
    end
    
    historyx = [historyx, x];
    historyy = [historyy, y];
    
    
    % Display position
    plot(xfin, yfin, 'ko', 'MarkerSize', 20)
    hold on
    plot(xmax/2, ymax/2, 'k+');
    plot(x, y, 'ko', 'MarkerFaceColor', [TimeLim-counter, counter, 0]/(2 * TimeLim) + 0.5, 'MarkerSize', 16)
    plot(PL(1), PL(2), 'b.');
    hold off
    text(15, ymax-35, 'ECRAN', 'FontName', 'Times New Roman', 'FontSize', 14, 'Color', [0.5,0.5,0.5]);
    text(xmax/2, ymax-35, [num2str(ttime), ' s'], 'FontName', 'Times New Roman', 'FontSize', 14, 'Color', [0.5, 0.5, 0.5]);
    axis([1, xmax, 1, ymax]);
    pause(Ts)
    
    ttime = ttime + Ts;
    if( sqrt( (x-xfin)^2 + (y-yfin)^2 ) < thresh )
        counter =  counter + Ts;
    else
        counter = 0;
    end
end
if(stopflag == true)
    if( sqrt( (x-xfin)^2 + (y-yfin)^2 ) < thresh )
        displaycolour = 'c';
    else
        displaycolour = 'm';
    end
    
    axes(handles.DisplayAxes);
    plot(xfin, yfin, 'ko', 'MarkerSize', 20)
    text(15, ymax-35, 'ECRAN', 'FontName', 'Times New Roman', 'FontSize', 14, 'Color', [0.5, 0.5, 0.5]);
    hold on
    plot(x, y, 'ko', 'MarkerFaceColor', displaycolour, 'MarkerSize', 16)
    hold off
    axis([1, xmax, 1, ymax]);
end
if( counter >= TimeLim )
    axes(handles.DisplayAxes);
    hold on
    plot(historyx, historyy, ':', 'color', [0.5, 0.5, 0.5])
    plot(x, y, 'ko', 'MarkerFaceColor', 'c', 'MarkerSize', 16)
    hold off
    set(handles.StartButton, 'Visible', 'on');
    set(handles.StopButton , 'Visible', 'off');
    stopflag = true;
    uiwait(msgbox({['Nivel terminat in ......... ', num2str(ttime - Ts),' secunde!'];'   '}, 'Bravo!', 'modal'));
    cmpls = num2str(ttime - Ts);
    uiresume;
end



% --- Executes on button press in StopButton.
function StopButton_Callback(hObject, eventdata, handles)
% hObject    handle to StopButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.StartButton, 'Visible', 'on');
set(handles.StopButton, 'Visible', 'off');
global stopflag;
stopflag = true;


% --- Executes on button press in HelpButton.
function HelpButton_Callback(hObject, eventdata, handles)
% hObject    handle to HelpButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
%path = matlab.desktop.editor.getActiveFilename
I = imread('Help.png');
figure, imshow(I)


% --- Executes on slider movement.
function LevelSlider_Callback(hObject, eventdata, handles)
% hObject    handle to LevelSlider (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

global level;
level = get(hObject, 'Value');
set(handles.Level, 'String', num2str(level - 1));


% --- Executes during object creation, after setting all properties.
function LevelSlider_CreateFcn(hObject, eventdata, handles)
% hObject    handle to LevelSlider (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor'))
    set(hObject, 'BackgroundColor', [.9 .9 .9]);
end

This is the script game that moves the mouse. Here is the PID controller implementation.

Maybe I m not implementig the pid correctly, or the k coeff are not the rigths one?

load('sharedData.mat','stopflag', 'xfin', 'yfin', 'Level', 'xmax', 'ymax');
import java.awt.Robot;
import java.awt.event.*;
robot = Robot();

Ts = 0.01;

error_x_prev = 0;
error_y_prev = 0;
integral_x = 0;
integral_y = 0;

myflag = false;

Ts = 0.01; % secunde
for d = 1:1250
if Level == 3
        disp("level 3")
        createReadLock();

        Kp =3;  % Proporțional
        Ki = 1.5; % Integral
        Kd = 10;  % Derivativ
        Ts = 0.01;
    
        if canRead
            % The 'writer' program is not accessing the data
            
            load('sharedData.mat', 'x', 'y'); % Read data from file
            
            dot_x = x
            dot_y = y
    
            % disp(x)
            % disp(y)
    
            deleteReadLock(); % Delete lock, so the 'writer' can access data
            
            %pause(0.5); % Wait before reading data again
        else
            % The 'writer' program is accessing the data
            
            deleteReadLock();
            
            % Wait briefly, to give the 'writer' time to finish using file
            % before trying again
            pause(0.1);
        end

        pos = get(0, 'PointerLocation');
        x_current = pos(1);
        y_current = pos(2);
    
        error_x = xfin - dot_x;
        error_y = yfin - dot_y;

        if myflag == false
            error_x_prev = error_x;
            error_y_prev = error_y;
            myflag = true;
        end
    
        % Proportional
        P_x = Kp * error_x;
        P_y = Kp * error_y;
    
        % Integral
        integral_x = integral_x + error_x * Ts;
        integral_y = integral_y + error_y * Ts;
        I_x = Ki * integral_x;
        I_y = Ki * integral_y;
    
        % Derivativ
        x_derivative = (error_x - error_x_prev) / Ts;
        y_derivative = (error_y - error_y_prev) / Ts;
        D_x = Kd * (error_x - error_x_prev) / Ts;
        D_y = Kd * (error_y - error_y_prev) / Ts;
    
        % PD TOTAL
        x_control = P_x + D_x + I_x;
        y_control = P_y + D_y + I_y;
    
        new_x = x + x_control;
        new_y = y + y_control;
    
        inv_y = ymax - new_y;
        
        robot.mouseMove(new_x, inv_y)
        
        error_x_prev = error_x;
        error_y_prev = error_y;
    
        if sqrt(error_x^2 + error_y^2) < 2 
            disp('Bila a ajuns la cerc!');
            robot.mouseMove(xmax/2, ymax/2);
            break;
        end
    
        % pause(Ts);

    end

Thanks for the help.