Uklizec - Franta & Franta

var HLEDEJ = 1;
var ROVNE = 2;
var CUKNI = 3;
var ZPATECKA = 4;
var OTOC180 = 5;
var STOP = 6;
var ZPET_PRI_ZPATECCE = 7;
var OTOC_PRI_ZPATECCE = 8;
var caraL = 0;
var caraR = 0;
var stav = STOP;
var vzdalenost = 0;
var dist = 0;
var casovac = 1;
var ultrazvuk;
var stopky = 0;
var minuleX=0;
function HCSRcb(x) {
    if (x<300) {
        vzdalenost = (x + minuleX) / 2;
        minuleX = x;
    }
}

ultrazvuk = (require("HC-SR04").connect(B11, B10, HCSRcb)); //PORT_IN3

function spustiDopravnik() {
    //pasovy dopravnik:
    analogWrite(E13, 0.05, {
        freq: 50
    });
    analogWrite(E14, 0.1, {
        freq: 50
    });
    //vyklopka vodorovne - do klidoveho stavu :
    analogWrite(B14, 0.09, {
        freq: 50
    });

}
function zastavDopravnik() {
    analogWrite(E13, 0.075, {
        freq: 50
    });
    analogWrite(E14, 0.075, {
        freq: 50
    });
}

function vyklop() {
    print('Vyklapim..');
    analogWrite(B14, 0.05, {
        freq: 50
    });
}
function rovne() {
    print('jedu rovne');
    analogWrite(E9, 0.05, {
        freq: 50
    });
    analogWrite(E11, 0.1, {
        freq: 50
    });
}
function zpet() {
    print('jedu zpet');
    analogWrite(E9, 0.1, {
        freq: 50
    });
    analogWrite(E11, 0.05, {
        freq: 50
    });
}
function otacejL() {
    print('otacim vlevo');
    analogWrite(E9, 0.05, {
        freq: 50
    });
    analogWrite(E11, 0.05, {
        freq: 50
    });
}
function skenuj() {
    print('Skenuji-hledam krabici');
    analogWrite(E9, 0.072, {
        freq: 50
    });
    analogWrite(E11, 0.072, {
        freq: 50
    });
}

function otacejR() {
    print('otacim vpravo');
    analogWrite(E9, 0.1, {
        freq: 50
    });
    analogWrite(E11, 0.1, {
        freq: 50
    });
}
function stojim() {
    print('stojim..');
    analogWrite(E9, 0.074, {
        freq: 50
    });
    analogWrite(E11, 0.074, {
        freq: 50
    });
    //vyklopka:
    analogWrite(B14, 0.05, {
        freq: 50
    });
    zastavDopravnik();
}

setWatch(function () {
    if (stav == STOP) {
        stav = HLEDEJ;
        stopky = 0;
        casovac=0;
        rovne();
        spustiDopravnik();
        digitalWrite(D9, false); //RED1 led
    } else {
        stav = STOP;
        stojim();
    }
}, "C5", {
    repeat: true,
    edge: 'rising',
    debounce: 100
});

setInterval(function () {
    caraL = digitalRead(C12) == 0; // PORT_IN1
    caraR = digitalRead(B7) == 0; // PORT_IN2
    caraBack = digitalRead(C10) === 0; // && digitalRead(C11) === 0; // nefunguje
    ultrazvuk.trigger();
    //print(stav);
    //print(vzdalenost);
    //print(caraL);
    //print(caraR);
    casovac++;
    stopky++;
    if (stav != STOP) {
        //print("Stav=" + stav + " casovac="+casovac+" stopky=" + stopky);
        if (stopky == 100) {    // po minute a pul zacni hledat krabici...
            digitalWrite(D9, true);
            stav = ZPATECKA;
            zastavDopravnik();
            otacejL();
        }
    }
    switch (stav) {
    case HLEDEJ:
        if (caraL) {
            otacejR();
            stav = OTOC180;
            casovac = 0;
        } else if (caraR) {
            otacejL();
            stav = OTOC180;
            casovac = 0;
        }
        break;
    case OTOC180:
        if (casovac == 10) {
            stav = HLEDEJ;
            rovne();
            casovac = 0;
        }
        break;
    case ZPATECKA:
        if (caraBack) {
            rovne();
            stav = ZPET_PRI_ZPATECCE;
            casovac = 0;
        } else if (vzdalenost < 10) {
            stav = STOP;
            stojim();
            vyklop();
        } else if (vzdalenost < 150) {
            zpet();
            casovac = 0;
        } else {
            skenuj();
            if (casovac > 50) {
                casovac = 0;
                zpet();
            }
        }
        break;
    case ZPET_PRI_ZPATECCE:
        if (casovac == 20) {
            stav=OTOC_PRI_ZPATECCE;
            otacejR();
            casovac = 0;
        }
        break;
    case OTOC_PRI_ZPATECCE:
        if (casovac == 20) {
            stav = ZPATECKA;
            zpet();
            casovac = 0;
        }
        break;
    }
}, 100);