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; } }