Robo.jc

#include "ASEIO.h"

/**
 * Executor blocks
 */
// void SetMoto(int id, int power);
// void printf(string text);
// void wait(float time);
// Set digital output
// void SetDO(int flags, int bool);
// void BEEP(float frequency, float time);
// void ServoPort_Init(int port);
// Digital Servo
// void PO16_Move(int id, int speed, int angleValue);
/**
 * Sensor blocks
 */
// Light/grayscale detection
// int AI(int port);
// float seconds();
// void resettime();
// Counter
// int encoder(int port);

// Motors
int ML = 0;     // Left
int MR = 1;     // Right
int MU = 2;     // Up

// Strengths
int SN = 85;    // Normal
int ST = 85;    // Turn
int SU = 100;    // Special motor

// nije crno < 1000 < jeste crno

void left() {
    SetMoto(ML, -ST);
    SetMoto(MR, -ST);
}

void right() {
    SetMoto(ML, ST);
    SetMoto(MR, ST);
}

void forward() {
    SetMoto(ML, SN);
    SetMoto(MR, -SN);
}

void backward() {
    SetMoto(ML, -SN);
    SetMoto(MR, SN);
}

void stop()
{
	SetMoto(ML, 0);
	SetMoto(MR, 0);
}

bool crno(int sensor)
{
    return AI(sensor) > 700;
}

void idiDoRaskrsnice()
{
    while((!crno(6) && !crno(10)))
    {
        if(crno(7) && crno(9)){ forward(); }
        else if (!crno(7) && !crno(9)){ forward(); }
        else if(crno(7) && !crno(9)){ left(); }
        else if(!crno(7) && crno(9)){ right(); }
    }
    stop();
}

void pravoNaR()
{
    forward();
    while(crno(6) || crno(10));
    stop();
}

void levoNaR()
{
    forward();
    wait(0.5);
    left();
    wait(0.8);
    while(crno(8));
    wait(0.2);
    while(!crno(8));
    stop();
}

void desnoNaR()
{
    forward();
    wait(0.5);
    right();
    wait(0.8);
    while(crno(8));
    wait(0.2);
    while(!crno(8));
    stop();
}

void start()
{
    forward();
    wait(0.5);
    stop();
        
}

void park()
{
    forward();
    wait(1);
    stop();
}

void o180()
{
    left();
    wait(1.5);
    while(!crno(8));
    stop();
}

void o180d()
{
    right();
    wait(1.5);
    while(!crno(8));
    stop();
}

void idiM(double vreme)
{
    forward();
    wait(vreme);
    stop();
}

void idi(double vreme)
{
	resettime();
	while(seconds()<=vreme)
	{
		if(crno(7) && crno(9)){ forward(); }
        else if (!crno(7) && !crno(9)){ forward(); }
        else if(crno(7) && !crno(9)){ left(); }
        else if(!crno(7) && crno(9)){ right(); }
	}
    stop();
}

void idina9()
{
    //USKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idi(2);
}

void btb9()
{
    //USKI DOLAZAK
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}

void idina8()
{
    //USKI START
    start();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idi(1);
}

void btb8()
{
    //USKI DOLAZAK
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    park();
}

void idina7()
{
    //USKI START
    start();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idi(2);
}

void btb7()
{
    //USKI DOLAZAK
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    park();
}

void idina6()
{
    //SIROKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idi(0.5);
}

void btb6()
{
    //SIROKI DOLAZAK
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}

void idina5()
{
    //USKI START
    start();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
}

void btb5()
{
    //USKI DOLAZAK
    backward();
    wait(1);
    stop();
    while((!crno(6) && !crno(10)))
    {
        if(crno(7) && crno(9)){ backward(); }
        else if (!crno(7) && !crno(9)){ backward(); }
        else if(crno(7) && !crno(9)){ right(); }
        else if(!crno(7) && crno(9)){ left(); }
    }
    o180();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    park();
}

void idina4()
{
    //SIROKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idi(2);
}

void btb4()
{
    //SIROKI DOLAZAK
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}

void idina3()
{
    //SIROKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idi(2);
}

void btb3()
{
    //SIROKI DOLAZAK
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}

void idina1()
{
    //SIROKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idi(2);
}

void btb1()
{
    //SIROKI DOLAZAK
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}

void idina2()
{
    //SIROKI START
    start();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idi(2);
}

void btb2()
{
    //SIROKI DOLAZAK
    idiDoRaskrsnice();
    desnoNaR();
    idiDoRaskrsnice();
    levoNaR();
    idiDoRaskrsnice();
    pravoNaR();
    idiDoRaskrsnice();
    park();
}
//koriguj 5 i 6
void main() {
    idina1();
    wait(1);
    o180();
    wait(1);
    btb1();
    wait(5);
    idina2();
    wait(1);
    o180();
    wait(1);
    btb2();
    wait(5);
    idina3();
    wait(1);
    o180();
    wait(1);
    btb3();
    wait(5);
    idina4();
    wait(1);
    o180();
    wait(1);
    btb4();
    wait(5);
    idina5();
    wait(1);
    o180();
    wait(1);
    btb5();
    wait(5);
    idina6();
    wait(1);
    o180();
    wait(1);
    btb6();
    wait(5);
    idina7();
    wait(1);
    o180();
    wait(1);
    btb7();
    wait(5);
    idina8();
    wait(1);
    o180d();
    wait(1);
    btb8();
    wait(5);
    idina9();
    wait(1);
    o180();
    wait(1);
    btb9();
}
//2013DUODUOProgramAA0905