clear all
clc
handle = COM_OpenNXT();
COM_SetDefaultNXT(handle);
col=["","","","";"","","","";"","","",""];
OpenSwitch(SENSOR_1);
OpenNXT2Color(SENSOR_2, 'FULL');
OpenNXT2Color(SENSOR_3, 'FULL');
OpenSwitch(SENSOR_4);

RLMotor=NXTMotor('A', 'Power', 0);
VHMotor=NXTMotor('C', 'Power', 0);
HAMotor=NXTMotor('B', 'Power', 0);

while GetSwitch(SENSOR_1)==0
    pause(0.2)
    disp('Knopf nicht betätigt')
end

for a=1:1
    RLMotor=NXTMotor('A', 'Power', 5);
    RLMotor.SendToNXT();
    disp('Motor gestartet')
    pause(2.5)
    color = GetNXT2Color(SENSOR_3);
    disp('Sensor gestartet')

    while strcmp(color,'BLACK') ~= 1
        disp('Falsche Farbe')
        pause(0.2)
        color = GetNXT2Color(SENSOR_3);
        disp(color)
    end
    
    RLMotor.Stop('brake', handle);
    disp('Markierung erkannt')
    disp(color)
    pause(1.0)
        
    for b=1:3
        HAMotor=NXTMotor('B','Power', 9);
        HAMotor.SendToNXT();
        pause(0.9)
        HAMotor.Stop('brake', handle);
        pause(0.5)
        col(b,a) = GetNXT2Color(SENSOR_2);
        pause(1.0)
        HAMotor=NXTMotor('B','Power', -9);
        HAMotor.SendToNXT();
        pause(1.0)
        HAMotor.Stop('brake', handle);
        
        if b==1 || b==2
            VHMotor=NXTMotor('C','Power', 7);
            VHMotor.SendToNXT();
            pause(1.5)
            VHMotor.Stop('brake', handle);
        elseif b==3
            VHMotor=NXTMotor('C','Power', -7);
            VHMotor.SendToNXT();
            pause(3.0)
            VHMotor.Stop('brake', handle);
        end
    end
end

RLMotor=NXTMotor('A', 'Power', -5);
RLMotor.SendToNXT();
color = GetNXT2Color(SENSOR_3);
while strcmp(color,'RED') ~= 1
    disp('Falsche Farbe')
    pause(0.2)
    color = GetNXT2Color(SENSOR_3);
    disp(color)
end
pause(5.0)

%Hier soll der Roboter die Paare finden

    
RLMotor.Stop('off', handle);
VHMotor.Stop('off', handle);
HAMotor.Stop('off', handle);

CloseSensor(SENSOR_1);
CloseSensor(SENSOR_2);
CloseSensor(SENSOR_3);
CloseSensor(SENSOR_4);
 
COM_CloseNXT('all')