%% moving forward to a fixed distance to the wall
close all; clear all; % close all figure windows and clear workspace

% set up connection to brick
COM_CloseNXT('all');  
hNXT=COM_OpenNXT(); 
COM_SetDefaultNXT(hNXT);

% Define motor command for forward movement
forward=NXTMotor('BC'); 
forward.Power=50; 
forward.TachoLimit=0; 

%define sensor on port 4
OpenUltrasonic(SENSOR_4); 

% plant parameters
d=10; e=1000;

%% negative feedback controler

setpoint=40;
while abs(e)>-1
    disToWall=GetUltrasonic(SENSOR_4)
    e=disToWall-setpoint
    forward.Power=50*sign(e);
     tic;
    forward.SendToNXT();
   
    while toc<abs(e)/d
    end
    forward.Stop('brake')
end


%%cleaning up
forward.Stop('off');
CloseSensor(SENSOR_1);
COM_CloseNXT(hNXT);

