void Calibrate()
{
T("enter Calibrate");
LLinit(LINELEADER);
nxtScrollText("Place over white");
PressBumperToContinue();
wait1Msec(500);
LLcalWhite(LINELEADER);
nxtScrollText("");
nxtScrollText("Place over line");
nxtScrollText("(Black)");
PressBumperToContinue();
wait1Msec(500);
LLcalBlack(LINELEADER);
// compass calibration
nxtScrollText("");
nxtScrollText("compass calibrate");
PressBumperToContinue();
wait1Msec(500);
motor[LEFT] = 30;
motor[RIGHT] = -30;
wait1Msec(500);
HTMCstartCal(COMPASS);
wait1Msec(5000);
HTMCstopCal(COMPASS);
motor[LEFT] = 0;
motor[RIGHT] = 0;
PlaySound(soundFastUpwardTones);
T("leave Calibrate");
wait1Msec(2000);
}
//************************
//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);
//=====================================
}
}
void AddBookToBst( BST bst, char* Book )
{
FILE *pFile;
char aux[WORDSIZE], *word, c;
int i, line;
long int pLine;
LL teste;
teste = LLinit();
pFile = fopen( Book ,"r" );
if ( pFile == NULL ) printf("Erro na abertura do Arquivo %s. \n", Book );
else
{
pLine = ftell(pFile);
i = line = 0;
c = fgetc( pFile );
while ( c != EOF )
{
if ( ( c >= 'A' && c <= 'Z' ) )
{
aux[i] = ( c - 'A' ) + 'a';
i++;
}
else if ( ( c >= 'a' && c <= 'z') )
{
aux[i] = c;
i++;
}
else if ( c == '\n' )
{
pLine = ftell(pFile);
line++;
i = 0;
}
else
{
word = malloc((i+1)*sizeof(char));
strncpy( word, aux, i );
word[i+1] = '\0';
if ( !BSTisthere( bst, word ) ) BSTAddNode( bst, word );
BSTPutInNode( bst, word, Book, line, pLine );
i = 0;
}
c = fgetc( pFile );
}
fclose(pFile);
}
}
void STACKput( Pilha p, LL l )
{
pRamo ramo;
ramo = malloc(sizeof(ramo));
if ( ramo == NULL )
{
printf("O ramo nao pode ser alocado. \n");
exit(0);
}
ramo->linked = LLinit();
ramo->linked = l;
ramo->Bef = p->Top;
p->Top = ramo;
(p->Tam)++;
LL STACKget( Pilha p )
{
LL l;
pRamo Aux;
if ( !STACKempty( p ) )
{
l = LLinit();
l = (p->Top)->linked;
Aux = p->Top;
p->Top = Aux->Bef;
(p->Tam)--;
free(Aux);
return l;
}
return NULL;
