#pragma config(Motor, motorA, MA, tmotorNormal, openLoop, encoder)

#pragma config(Motor, motorB, , tmotorNormal, openLoop, encoder)

#pragma config(Motor, motorC, MC, tmotorNormal, openLoop, encoder)

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/*

************************************************

******************* PORTAS DOS SENSORES

************************************************

*/

tSensors PORT_ARD = S3;

tSensors PORT_COMP = S1;

tSensors PORT_ACC = S4;

tSensors PORT_LL = S2;

//tSensors PORT_T1 = S3;

//tSensors PORT_T2 = S3;

/*

************************************************

******************* DEFINICOES GERAIS

************************************************

*/

#define BT_LEFT 2

#define BT_ENTER 3

#define BT_RIGHT 1

/*

************************************************

******************* INCLUDES

************************************************

*/

//Sensores

#include "HTMC-driver.h"

#include "HTAC-driver.h"

#include "ARDU-driver.h"

#include "MSLL-driver.h"

//Commom

#include "INC-ERRO_HANDLE.c"

#include "INC-CONFIG_TESTS.c"

#include "INC-Controle.c"

#include "INC-Move.c"

#include "INC-ShowGraphic.c"

#include "DL-lib.c"

/*

************************************************

******************* FUNCOES DE ESTADOS (SETUP)

************************************************

*/

#define ST_INIT 0

#define ST_CONFIG 1

#define ST_WAIT 2

#define ST_LINHA 3 //MT_ACCEL

#define ST_GAP 4 //MT_ACCEL MT_TOQUE

#define ST_OBSTACULO 5

#define ST_SOBE 6

#define ST_ENTRA 7

#define ST_RADAR 8

#define ST_LATA 9

#define ST_DEPOSITA 10

int P_ESTADO_MAX = 10;

string STATE_TABLE[11] = {

"INIT",

"CONFIG",

"WAIT",

"LINHA",

"GAP",

"OBSTACULO",

"SOBE",

"ENTRA",

"RADAR",

"LATA",

"DEPOSITA"};

#include "INC-STATES.c"

/*

************************************************

******************* VARIAVEIS PING DE SENSORES

************************************************

*/

//Configurao dos sensores

#define ACCEL_AXIS_RAMPA 1

#define ACCEL_DELTA 40

int ACCEL_Offset = 0;

//Valores dos sensores

int ACCEL[3] = {0,0,0};

int ACCEL_Filter = 0;

bool ACCEL_Rampa = false;

int Touch = 0;

tByteArray USwall;

int LL_Avr = 0;

int LL_IO = 0;

/*

************************************************

******************* FUNCOES DE MULTI-TAREFA

************************************************

*/

#define MT_DEFAULT 0

#define MT_BEEP 1

#define MT_STOP_BUTTON 2

#define MT_ACCEL 3

#define MT_TOQUE 4

#define MT_LL 5

#define MT_US 6

sub MTASK_DOTASK(int MTASK_ID){

switch (MTASK_ID)

{

//********

case MT_DEFAULT:

wait1Msec(1);

break;

//********

case MT_BEEP:

PlayTone(200, 12);

wait10Msec(120);

break;

//********

case MT_STOP_BUTTON:

if(nNxtButtonPressed==BT_ENTER)

{

int static TimeDif;

TimeDif=time10[T4];

while(nNxtButtonPressed==BT_ENTER){

if(time10[T4]-TimeDif>50)

{

MV_StopMotors();

ClearSounds();

PlaySound(soundBlip);

ESTADO_SET_TARGET(ST_WAIT);

break;

}

}

}

break;

//********

case MT_ACCEL:

if(!getSafe(3))

{

hogCPU();

HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);

releaseCPU();

if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){

if (ACCEL_Filter++ > 2){

ACCEL_Rampa = true;

ACCEL_Filter = 3;

}

}else{

if (ACCEL_Filter-- < 2){

ACCEL_Filter = 0;

ACCEL_Rampa = false;

}

}

setSafe(3,300);

}

break;

//********

case MT_TOQUE:

wait1Msec(1);

break;

//********

case MT_LL:

hogCPU();

LL_Avr = LLreadAverage(PORT_LL);

LL_IO = LLreadResult(PORT_LL);

releaseCPU();

wait1Msec(5);

break;

//********

case MT_US:

wait10Msec(3);

hogCPU();

ReadAllUS_short(PORT_ARD,USwall);

releaseCPU();

break;

}

}

#include "INC-MULTI_TASKING.c"

/*

************************************************

******************* FUNCOES DE ESTADOS (FUNCOES)

************************************************

*/

//************************

// Funcoes a serem rodadas

// 1 vez no comeco do

// programa.

//************************

void F_STATE_INIT()

{

MTASK_SET_RUN();

while(ESTADO_IS_CURRENT())

{

//=====================================

ESTADO_SET_TARGET(ST_CONFIG);

//=====================================

}

}

//************************

//Configura sensores e joga erros na tela

//************************

void F_STATE_CONFIG()

{

MTASK_SET_RUN(MT_BEEP);

//Enquanto o estado atual for o estado target

eraseDisplay();

nxtDisplayCenteredBigTextLine(3, "WAIT");

while(ESTADO_IS_CURRENT())

{

//=====================================

wait10Msec(100);

//Seta os tipos de porta no NXT

SensorType[PORT_ARD] = sensorI2CCustom;

SensorType[PORT_ACC] = sensorI2CCustom;

SensorType[PORT_COMP] = sensorI2CCustom;

//Inicia Arduino

while(!ERRO_SET_CODE(" CF: ARDU ERR",ARDUinit(PORT_ARD)));

//Inicia Acc Sensor

while(!ERRO_SET_CODE(" CF: ACC ERR",HTACinit(PORT_ACC)));

HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);

ACCEL_Offset=ACCEL[ACCEL_AXIS_RAMPA];

//Inicia Line Leader

while(!ERRO_SET_CODE(" CF: LL ERR",LLinit(PORT_LL)));

//Inicia Bussola

while(!ERRO_SET_CODE(" CF: COMP ERR",HTMCreadHeading(PORT_COMP)>=0));

//Testa motores

while(!ERRO_SET_CODE(" CF: MOTOR A",TEST_MOTOR(MA)));

while(!ERRO_SET_CODE(" CF: MOTOR C",TEST_MOTOR(MC)));

bMotorReflected[MA] = true;

bMotorReflected[MC] = true;

GARRA_H(PORT_ARD, GARRA_H_FECHA);

wait10Msec(20);

GARRA_V(PORT_ARD,GARRA_V_SOBE);

wait10Msec(30);

GARRA_V(PORT_ARD,GARRA_V_PARA);

GARRA_H(PORT_ARD, GARRA_H_PARA);

setSafe(3,300);

setSafe(4,300);

ESTADO_SET_TARGET(ST_WAIT);

//=====================================

}

}

//************************

//Pausa e despausa programa

//-= < Para iniciar maze

//-= > Para ultima sala

//************************

void F_STATE_WAIT()

{

MTASK_SET_RUN(MT_DEFAULT,MT_ACCEL,MT_US);

eraseDisplay();

while(ESTADO_IS_CURRENT())

{

//=====================================

motor[MA]=0;

motor[MC]=0;

nxtDisplayCenteredBigTextLine(4, "MENU");

nxtDisplayString(7, " LINHA - ENTRA");

while(nNxtButtonPressed != BT_LEFT && nNxtButtonPressed != BT_RIGHT){

nxtDisplayCenteredTextLine(1, "R=%i T=%i", ACCEL_Rampa, Touch == 0);

// Le arduino para pingar os ultrasons

//ReadAllUS_short(PORT_ARD,USwall);

nxtDisplayCenteredTextLine(2, "%3i %3i %3i", USwall[0],USwall[1],USwall[2]);

}

if(nNxtButtonPressed==BT_LEFT){

ESTADO_SET_TARGET(ST_LINHA);

}else{

ESTADO_SET_TARGET(ST_RADAR);

}

while(nNxtButtonPressed!=-1);

PlaySound(soundBlip);

eraseDisplay();

//=====================================

}

}

int CountBits(byte Counting){

int Ret = 0;

for(int k = 0; k < 8; k++){

if(((0b10000000 >> k )& Counting)>0){

Ret++;

writeDebugStream("1");

}else{

writeDebugStream("0");

}

}

writeDebugStreamLine("Ret = %i", Ret);

return Ret;

}

//************************

// Segue Linha

//************************

void F_STATE_LINHA()

{

MTASK_SET_RUN(MT_STOP_BUTTON,MT_LL,MT_ACCEL);

float Error = 0;

float Integral = 0;

float Out = 0;

MV_StopMotors();

wait10Msec(2);

while(ESTADO_IS_CURRENT())

{

//=====================================

//int Error = LLreadAverage(S1)-45;

wait10Msec(2);

Error = LL_Avr - 45;

if(Error ==-45){

Error = 0;

PlayTone(200,10);

ESTADO_SET_TARGET(ST_GAP);

}

nxtDisplayStringAt(20, 20, "%i",Error);

Integral += Error*0.004;

Out = Error*1.5 + Integral;

motor[motorA]=35 + Out;

motor[motorC]=35 - Out;

//Se detectou rampa com acceleracao, sobe a rampa

if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);

//=====================================

}

}

int TryAlign = 0;

//************************

// Passa do GAP

//************************

void F_STATE_GAP()

{

MTASK_SET_RUN(MT_ACCEL,MT_LL,MT_STOP_BUTTON);

int MaxCount = 0;

int MaxValue = 0;

byte MaxIO = 0;

int Sidec = 0;

/*

* Checa se foi uma curva de 90o

* e se alinha na linha

*/

nMotorEncoder[MA] = 0;

nMotorEncoder[MC] = 0;

setSafe(1,5000);

for(int h = 0; h < 65; h++){

while(getSafe(1) && abs(nMotorEncoder[MA]) <h && !ACCEL_Rampa){

motor[MA]=-60;

motor[MC]=0;

}

MV_StopMotors();

while(getSafe(1) && abs(nMotorEncoder[MC]) <h && !ACCEL_Rampa){

motor[MA]=0;

motor[MC]=-60;

}

MV_StopMotors();

byte LLSnapIO = LL_IO;

byte LLSnapAV = LL_Avr;

int LLONCount = CountBits(LLSnapIO);

if((LLSnapIO & 128) > 0){

writeDebugStreamLine("<");

MaxCount = 5;

Sidec = 1;

}else if((LLSnapIO & 1) > 0){

writeDebugStreamLine(">");

MaxCount = 5;

Sidec = -1;

}else if(LLONCount > MaxCount){

MaxCount = LLONCount;

MaxValue = LLSnapAV - 45;

MaxIO = LLSnapIO;

}

}

writeDebugStreamLine("%i - %i",Sidec,1 & 0b00111111);

writeDebugStreamLine("MaxC =%i | MaxA = %i",MaxCount,MaxValue);

nxtDisplayBigTextLine(6, "%i",MaxCount);

if(MaxCount>4){

hogCPU();

HTMCsetTarget(PORT_COMP);

releaseCPU();

setSafe(1,5000);

nMotorEncoder[MA] = 0;

nMotorEncoder[MC] = 0;

int h =0;

while(LL_Avr > 0 && getSafe(1) && abs(nMotorEncoder[MA]) <80){

while(getSafe(1) && abs(nMotorEncoder[MA]) <h && !ACCEL_Rampa){

motor[MA]=50;

motor[MC]=0;

}

MV_StopMotors();

while(getSafe(1) && abs(nMotorEncoder[MC]) <h && !ACCEL_Rampa){

motor[MA]=0;

motor[MC]=50;

}

MV_StopMotors();

h++;

}

MV_Reto(70,10);

int Side = MaxValue/abs(MaxValue);

if(Sidec!=0) Side = Sidec;

/*if(0b10000000 & MaxIO > 0){

Side = 1;

}else if(0b00000001 & MaxIO > 0){

Side = -1;

}*/

while(abs(LL_Avr-45) > 20 && getSafe(1) && !ACCEL_Rampa){

nMotorEncoder[MA] = 0;

while(getSafe(1) && abs(nMotorEncoder[MA]) <2 && !ACCEL_Rampa){

motor[MA]=100 * Side;

motor[MC]=0;

}

MV_StopMotors();

nMotorEncoder[MC] = 0;

while(getSafe(1) && abs(nMotorEncoder[MC]) <2 && !ACCEL_Rampa){

motor[MA]=0;

motor[MC]=-100 * Side;

}

MV_StopMotors();

}

hogCPU();

int DeltaAngle = abs(HTMCreadRelativeHeading(PORT_COMP));

releaseCPU();

nxtDisplayBigTextLine(4, " %i", DeltaAngle);

if(DeltaAngle > 110){

MV_Reto(50,70);

MV_Spin(-50*Side,100);

wait10Msec(100);

setSafe(1,5000);

while(abs(LL_Avr-45) > 20 && getSafe(1) && !ACCEL_Rampa){

nMotorEncoder[MA] = 0;

while(getSafe(1) && abs(nMotorEncoder[MA]) <2 && !ACCEL_Rampa){

motor[MA]=-100 * Side;

motor[MC]=0;

}

MV_StopMotors();

nMotorEncoder[MC] = 0;

while(getSafe(1) && abs(nMotorEncoder[MC]) <2 && !ACCEL_Rampa){

motor[MA]=0;

motor[MC]=100 * Side;

}

MV_StopMotors();

}

}

ESTADO_SET_TARGET(ST_LINHA);

}else{

MV_Reto(50,110);

}

wait10Msec(30);

/*setSafe(2,2000);

MC_enc = nMotorEncoder[MC];

motor[MA] = 0;

motor[MC] = 50*ZZDir;

while(getSafe(1) && abs(nMotorEncoder[MC] - MC_enc) < Move/2 && !ACCEL_Rampa){

if(LL_Avr > 0){

ESTADO_SET_TARGET(ST_LINHA);

break;

}

}

MV_StopMotors();*/

while(ESTADO_IS_CURRENT())

{

//=====================================

motor[MA] = 70;

motor[MC] = 70;

//Faz um zigue zague

/*MA_enc = nMotorEncoder[MA];

motor[MA] = 50*ZZDir;

motor[MC] = 0;

while(getSafe(1) && abs(nMotorEncoder[MA] - MA_enc) < Move && !ACCEL_Rampa){

if(LL_Avr > 0){

ESTADO_SET_TARGET(ST_LINHA);

break;

}

}

MV_StopMotors();

MC_enc = nMotorEncoder[MC];

motor[MA] = 0;

motor[MC] = 50*ZZDir;

while(getSafe(1) && abs(nMotorEncoder[MC] - MC_enc) < Move && !ACCEL_Rampa){

if(LL_Avr > 0){

ESTADO_SET_TARGET(ST_LINHA);

break;

}

}

MV_StopMotors();*/

if(LL_Avr > 0)ESTADO_SET_TARGET(ST_LINHA);

//Se detectou rampa com acceleracao, sobe a rampa

if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);

//=====================================

}

}

//************************

// Sobe a Rampa

//************************

void F_STATE_SOBE()

{

PlayTone(500,10);

MTASK_SET_RUN(MT_STOP_BUTTON,MT_ACCEL,MT_US);

int USminDist = 999, USdist,USdelta,Lado1,Lado2;

float Error = 0, vI = 0, vP = 0, vD = 0, kI = 0.3, kP = 1.1, kD = 1, lD=4;

wait10Msec(200);

if(!ACCEL_Rampa) ESTADO_SET_TARGET(ST_LINHA);

MV_StopMotors();

wait10Msec(100);

hogCPU();

GARRA_V(PORT_ARD,GARRA_V_DESCE);

releaseCPU();

wait10Msec(50);

hogCPU();

GARRA_V(PORT_ARD,GARRA_V_PARA);

releaseCPU();

time1[T1]=0;

bool CLAW = true;

setSafe(4,5000);

while(ESTADO_IS_CURRENT())

{

//=====================================

// Le arduino para pingar os ultrasons

wait10Msec(3);

//ReadAllUS_short(PORT_ARD,USwall);

// Filtra dados

//// Seta as variaveis locais como locais

//// para nao dar bug (alterar o valor na outra task)

Lado1=USwall[0];

Lado2=USwall[2];

//// Acha a menor distancia que o robo fica das paredes

//if(Lado1+Lado2<USminDist)USminDist=USdist;

//// Filtra paredes (existe ou nao)

if(Lado1>60)Lado1=60-Lado2;

if(Lado2>60)Lado2=60-Lado1;

// Calcula o Error

vD = time1[T1] * ((Lado1 - Lado2) - vD) / 1000;

lD = vD;

vP = (Lado1 - Lado2);

vI += time1[T1] * vP / 1000;

//Aplica constantes

vP *= kP;

vI *= kI;

vD *= kD;

if(vI>20) vI = 20;

if(vI<-20) vI = -20;

Error = vP+

vI-

vD;

time1[T1]=0;

// Aplica erro aos motores

motor[MA]=85 - Error;

motor[MC]=85 + Error;

//Mostra no display os valores P, I, D e Stering final (Error)

eraseDisplay();

nxtDisplayString(2,"P = %i", (int)vP);

nxtDisplayString(3,"I = %i", (int)vI);

nxtDisplayString(4,"D = %i", (int)vD);

nxtDisplayBigStringAt((46 - Error)/2, 24, "|");

if(USwall[1] <100 && CLAW && !getSafe(5)){

MV_StopMotors();

wait10Msec(50);

hogCPU();

GARRA_V(PORT_ARD,GARRA_V_SOBE);

releaseCPU();

wait10Msec(50);

hogCPU();

GARRA_V(PORT_ARD,GARRA_V_PARA);

releaseCPU();

CLAW=false;

}

//Se saiu da rampa com acceleracao, entra na sala

if(!ACCEL_Rampa)ESTADO_SET_TARGET(ST_ENTRA);

//=====================================

}

}

//************************

// Entra na Sala

//************************

void F_STATE_ENTRA()

{

PlayTone(500,10);

MTASK_SET_RUN(MT_STOP_BUTTON,MT_US);

while(ESTADO_IS_CURRENT())

{

//=====================================

MV_StopMotors();

GARRA_V(PORT_ARD,GARRA_V_SOBE);

wait10Msec(50);

GARRA_V(PORT_ARD,GARRA_V_PARA);

setSafe(0,5000);

MV_Reto_Unlimited(60);

while(getSafe(0) && !ReadTouch(PORT_ARD)==0);

MV_Reto(80,0,7);

MV_Reto(-60,50);

MV_Spin(70,150);

MV_Reto(-60,0,20);

// Volta at ler a borda

while(USwall[2] > 200){

MV_Reto(-60,1);

}

MV_Vira(60,50,MC);

MV_Reto(60,300);

MV_Vira(-60,50,MC);

MV_Reto(-60,150);

MV_Vira(60,20,MC);

MV_Vira(80,180,MC);

ESTADO_SET_TARGET(ST_RADAR);

//=====================================

}

}

#define LATA_LOWER -16

#define LATA_HIGHER 16

#define LATA_LIMIT 110 //Limite de largura da lata

#define BASE_MIN 1300

#define LATA_DIST 300

int Base = 0;

#define WAY_A 1

#define WAY_B 2

#define WAY_AA 3

#define WAY_AB 4

#define WAY_AC 5

int RelativeCompass = 0;

void F_STATE_RADAR()

{

MTASK_SET_RUN();

RelativeCompass = HTMCreadHeading(PORT_COMP);

HTMCsetTarget(PORT_COMP);

while(ESTADO_IS_CURRENT())

{

//=====================================

int US_Raw[400];

int US_Dif[400];

int Sample = 0;

int Step = 5;

tByteArray USRead;

tByteArray USRead1;

tByteArray USRead2;

tByteArray USRead3;

ReadAllUS(PORT_ARD,USRead);

wait10Msec(3);

/*for(sample = 0; sample<180; sample++){

MV_Vira(-80,1,MC);

ReadAllUS(PORT_ARD,USRead);

wait10Msec(3);

AUlt[sample]=USRead[1];

}

SG(sample);*/

nMotorEncoder[MC]=0;

nMotorEncoder[MA]=0;

bool exit = false;

while(!ReadTouch(PORT_ARD)==0 && !exit){

setSafe(0,5000);

motor[MA] = 100;

motor[MC] = 100;

while(nMotorEncoder[MC] < Sample*Step && getSafe(0)){

}

motor[MA] = 0;

motor[MC] = 0;

ReadAllUS(PORT_ARD,USRead1);

wait10Msec(3);

ReadAllUS(PORT_ARD,USRead2);

USRead[2] = min2(USRead1[2],USRead2[2]);

US_Raw[sample]=USRead[2];

writeDebugStreamLine("%i",US_Raw[Sample]);

Sample++;

if(!getSafe(0)) exit=true;

}

DL_Init();

DL_Insert("INIT");

//cria o grafico de diferenca

for(int i = 0; i < Sample - 1; i++){

US_Dif[i] = US_Raw[i+1] - US_Raw[i];

DL_Insert((string)US_Dif[i]);

}

DL_Insert("END");

DL_Insert("INIT");

int LatasPicoL [10];

int LatasPicoH [10];

int LatasWidth [10];

int LatasCenter [10];

int LataAtual = 0;

int ChoosenObject=-1;

//Acha objetos

for(int i=0; i<Sample; i++){

if(US_Dif[i]<LATA_LOWER){

DL_Insert("LATA "+(string)LataAtual);

LatasPicoL[LataAtual] = i;

while(i <Sample && US_Dif[i] < LATA_LOWER){

i++;

}

i = min2(Sample, i+10);

while(i <Sample && US_Dif[i] < LATA_HIGHER && LatasPicoL[LataAtual]+LATA_LIMIT >= i){

i++;

}

LatasPicoH[LataAtual] = i;

i = min2(Sample, i+10);

LatasCenter[LataAtual] = (LatasPicoL[LataAtual]+LatasPicoH[LataAtual])/2;

LatasWidth[LataAtual] = abs(LatasPicoL[LataAtual]-LatasPicoH[LataAtual]);

nxtDisplayTextLine(LataAtual+1, "S=%i E=%i W=%i",LatasPicoL[LataAtual],LatasPicoH[LataAtual],LatasWidth[LataAtual]);

DL_Insert("S= "+LatasPicoL[LataAtual]+" E= "+LatasPicoH[LataAtual]+" W="+LatasWidth[LataAtual]);

LataAtual++;

}

}

//Filtra objetos por Largura

int MaxWidth = 0;

for(int i=0; i<LataAtual; i++){

if(LatasWidth[i] >MaxWidth){

MaxWidth = LatasWidth[i];

ChoosenObject = i;

}

}

int Case = 0;

if(ChoosenObject>=0){

DL_Insert("Escolhida: " + ChoosenObject);

DL_Insert("Samples: " + Sample);

DL_Insert("Centro: " + LatasCenter[ChoosenObject]);

DL_Insert("Andada: "+((Sample - LatasCenter[ChoosenObject]) * Step));

}

if(nMotorEncoder[MC] < BASE_MIN && ChoosenObject < 0){

Case = WAY_B;

}else if(ChoosenObject >=0){

Case = WAY_A;

}else{

}

switch (Case){

case WAY_A:

int Side;

if(Sample/2 > LatasCenter[ChoosenObject]){

Side = 1;

}else{

Side = -1;

}

nxtDisplayBigTextLine(6, "WAY_A:%i",Side);

DL_Insert("WAY_A:"+Side);

int ToGo = ((Sample - LatasCenter[ChoosenObject]) * Step)-LATA_DIST*Side;

MV_Reto(-70,ToGo);

/*

* Vira 90 graus e se alinha a parede

*/

MV_Vira(80,310,MA);

MV_Reto(-80,0,10);

HTMCsetTarget(PORT_COMP);

int Error=0;

ReadAllUS(PORT_ARD,USRead);

wait10Msec(3);

while(!ReadTouch(PORT_ARD)==0){

ReadAllUS(PORT_ARD,USRead);

wait10Msec(3);

if(USRead[0]<20 || USRead[2]<20){

int Min = min2(USRead[0],USRead[2]);

if(Min > 10){

}else{

break;

}

}

Error = HTMCreadRelativeHeading(PORT_COMP) * 3;

motor[MA] = 50 - Error;

motor[MC] = 50 + Error;

}

MV_StopMotors();

break;

case WAY_B:

nxtDisplayBigTextLine(6, "WAY_B");

DL_Insert("WAY_B");

break;

}

DL_Close();

while(nNxtButtonPressed != BT_ENTER);

ESTADO_SET_TARGET(ST_WAIT);

//=====================================

}

}

/*

************************************************

******************* TASK MAIN

************************************************

*/

task main(){

StartTask(MULTI_TASKING);

while(1)

{

//Seleciona estado atual

ESTADO_SET_ESTADO();

nxtDisplayTextLine(0, "ST= %s", STATE_TABLE[G_ESTADO_TARGET]);

switch(G_ESTADO_TARGET)

{

//Funcoes realizadas somente no comeco do programa

case ST_INIT:

F_STATE_INIT();

break;

//Configura sensores

case ST_CONFIG:

F_STATE_CONFIG();

break;

//Pausa e despausa programa

case ST_WAIT:

F_STATE_WAIT();

break;

case ST_LINHA:

F_STATE_LINHA();

break;

case ST_GAP:

F_STATE_GAP();

break;

case ST_OBSTACULO:

F_STATE_CONFIG();

break;

case ST_SOBE:

F_STATE_SOBE();

break;

case ST_ENTRA:

F_STATE_ENTRA();

break;

case ST_RADAR:

F_STATE_RADAR();

break;

case ST_LATA:

F_STATE_CONFIG();

break;

case ST_DEPOSITA:

F_STATE_CONFIG();

break;

default:

ERRO_SET_CODE("ESTADO INEXISTENTE");

break;

}

}

}