LUX_INLINE operator T*(){
#ifdef _DEBUG
T* ptr;
booln res = getSafe(&ptr);
LUX_ASSERT(res);
return ptr;
#else
return getPtr();
#endif
}
//************************
// Entra na Sala
//************************
void F_STATE_ENTRA()
{
PlayTone(500,10);
MTASK_SET_RUN(MT_STOP_BUTTON);
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,100);
ESTADO_SET_TARGET(ST_WAIT);
//=====================================
}
}
//************************
// Passa do GAP
//************************
void F_STATE_GAP()
{
MTASK_SET_RUN(MT_ACCEL,MT_LL,MT_STOP_BUTTON);
int ZZDir = 1;
int Move = 90;
int MA_enc;
int MC_enc;
/*
* Checa se foi uma curva de 90o
* e se alinha na linha
*/
if(getSafe(2)){
TryAlign++;
}else{
TryAlign = 0;
}
setSafe(1,4000);
if(TryAlign==0){
nMotorEncoder[MA] = 0;
nMotorEncoder[MC] = 0;
for(int i = 0; i < 90; i++){
while(getSafe(1) && abs(nMotorEncoder[MA]) <i && !ACCEL_Rampa){
motor[MA]=-50;
motor[MC]=0;
}
while(getSafe(1) && abs(nMotorEncoder[MC]) <i && !ACCEL_Rampa){
motor[MA]=0;
motor[MC]=-50;
}
MV_StopMotors();
if(LL_Avr > 0){
MV_Reto(-50,40);
break;
}
}
}
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())
{
//=====================================
//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(!getSafe(1)){
ZZDir = ZZDir*-1;
setSafe(1,5000);
}
//Se detectou rampa com acceleracao, sobe a rampa
if(ACCEL_Rampa)ESTADO_SET_TARGET(ST_SOBE);
//=====================================
}
}
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;
}
}
