
COM_CloseNXT('all'); %cleans up workspace
close all;
clear all;
hNXT=COM_OpenNXT('bluetooth.ini'); % initiates NXT, hNXT is an arbitrar y name
COM_SetDefaultNXT(hNXT); %sets default handle
top=NXTMotor('c'); 
bottom=NXTMotor('b'); 
OpenSwitch(SENSOR_1); 
hold on;

for counter = 1:100
    counter
    %The angles for both wheels are read
    bottomDeg=bottom.ReadFromNXT.Position;
    topDeg=top.ReadFromNXT.Position;
    %the angles are then stored in angleWheel
    angleWheel(1,counter)=bottomDeg;
    angleWheel(2,counter)=topDeg;
    %This plot will show all the angles read from the
    %wheels, because hold is on
    plot(topDeg,bottomDeg,'o');
    %If the switch is pushed then pause the angle reader
    if GetSwitch(SENSOR_1)==1
        disp('paused');
        pause(4);
        disp('unpaused');
    end
end


CloseSensor(SENSOR_1); %properly closes the sensor
COM_CloseNXT(hNXT); % properly closes the NXT


