clear;

ev3=legoev3('usb');
motorL=motor(ev3,'A');
motorR=motor(ev3,'B');

trackspeed =30;

motorL.Speed = 20;
motorR.Speed = 20;

lineSensor = colorSensor(ev3);

%lineref = 18;
linemax = 56;
linemin = 3;

lineref = 18;

Kp = 10;
Kp_positive = 1.5;
Kp_negative = 3;
Ki = 0;
Kd = 0;

setpoint = lineref;

start(motorL);
start(motorR);

while 1 %~readButton(ev3,'center')

    %ls=readInputDeviceREADY_RAW(ev3,4,0,1); %read port 4
    sensor = readLightIntensity(lineSensor,'reflected');
    error = sensor - setpoint;

    if error > 0
        turn = Kp_positive*error;
    else
        turn = Kp_negative*error;
    end

    motorL.Speed = trackspeed - turn;
    if abs(motorL.Speed) > 100
        motorL.Speed = 100 * sign(motorL.Speed);
    end
    motorR.Speed = trackspeed + turn;
    if abs(motorR.Speed) > 100
        motorR.Speed = 100 * sign(motorR.Speed);
    end
end