static GUI_HMEM _Alloc(int size) { GUI_HMEM hMemNew, hMemIns; CheckInit(); size = Size2LegalSize(size); /* Check if memory is available at all ...*/ if (size > GUI_ALLOC.NumFreeBytes) { GUI_DEBUG_WARN1("GUI_ALLOC_Alloc: Insufficient memory configured (Trying to alloc % bytes)", size); return 0; } /* Locate free handle */ if ((hMemNew = FindFreeHandle()) == 0) return 0; /* Locate or Create hole of sufficient size */ hMemIns = FindHole(size); #if GUI_ALLOC_AUTDEFRAG if (hMemIns == -1) { hMemIns = CreateHole(size); } #endif /* Occupy hole */ if (hMemIns==-1) { GUI_DEBUG_ERROROUT1("GUI_ALLOC_Alloc: Could not allocate %d bytes",size); return 0; } { int Off = aBlock[hMemIns].Off+aBlock[hMemIns].Size; int Next = aBlock[hMemIns].Next; aBlock[hMemNew].Size = size; aBlock[hMemNew].Off = Off; if ((aBlock[hMemNew].Next = Next) >0) { aBlock[Next].Prev = hMemNew; } aBlock[hMemNew].Prev = hMemIns; aBlock[hMemIns].Next = hMemNew; } /* Keep track of number of blocks and av. memory */ GUI_ALLOC.NumUsedBlocks++; GUI_ALLOC.NumFreeBlocks--; if (GUI_ALLOC.NumFreeBlocksMin > GUI_ALLOC.NumFreeBlocks) { GUI_ALLOC.NumFreeBlocksMin = GUI_ALLOC.NumFreeBlocks; } GUI_ALLOC.NumUsedBytes += size; GUI_ALLOC.NumFreeBytes -= size; if (GUI_ALLOC.NumFreeBytesMin > GUI_ALLOC.NumFreeBytes) { GUI_ALLOC.NumFreeBytesMin = GUI_ALLOC.NumFreeBytes; } /* In order to be on the safe side, zeroinit ! */ memset(HMEM2PTR(hMemNew), 0, size); return hMemNew; }
RobotCommand TacticPush2Goal::getCommand() { oppIsValid = wm->ourRobot[0].isValid;//wm->theirRobot.IsValid;// opposite robot addData(); sortData(); if(!oppIsValid) opp = Vector2D(1000000,1000000); opp = wm->ourRobot[0].pos.loc;//wm->theirRobot.position;//wm->ourRobot[8].pos.loc; OppIsKhoraak=!circularBorder2.contains(opp);//out of his field bool reach=false; Avoided=false; AllIn=true; AnyIn=false; AllInCorner=true; AllUnAccessible=true; RobotCommand rc; if(!wm->ourRobot[id].isValid) return rc; rc.fin_pos.loc=Vector2D(400,0);//circularBorder.center(); rc.maxSpeed = 1.2; index=-1; int tah=balls.size()-1; if(tah != -1) { rc.fin_pos.loc=circularBorder2.nearestpoint(balls.at(tah)->pos.loc); rc.fin_pos.dir=(circularBorder.center()-rc.fin_pos.loc).dir().radian(); } FindBall(); // find approtiate ball if( index != -1 ) { point2 = balls.at(index)->pos.loc; FindHole(); Vector2D nrstpnt = (circularBorder.center()-point2); //nearest Point nrstpnt=nrstpnt.setLength(nrstpnt.length()-circularBorder2.radius()); diff2 = nrstpnt; nrstpnt=point2+nrstpnt; // state2=0; switch(state) { case 0:{ //Go Behind the Object vec2goal.setLength(250); // qDebug()<<"VEC 2 GOAL LENGTH = " << vec2goal.length(); rc.maxSpeed=1.3; rc.useNav = false;//true; rc.isBallObs = false;//true; rc.isKickObs = false;//true; rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 - vec2goal, rc.maxSpeed,balls.at(index)->vel.loc);//point2 - vec2goal; // wm->kn->PredictDestination() int rad = 150+ROBOT_RADIUS; Circle2D c(point2,rad); rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc); // if(Avoided) rc.maxSpeed=rc.maxSpeed; rc.fin_pos.dir=vec2goal.dir().radian(); // KeepInField(rc); reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,100); if(reach && !Avoided) state = 1; } break; case 1:{//Push rc.useNav = false; rc.isBallObs = false; rc.isKickObs = false; rc.maxSpeed=1.1; vec2goal.setLength(100); rc.fin_pos.loc=wm->kn->PredictDestination(wm->ourRobot[id].pos.loc,point2 + vec2goal, rc.maxSpeed,balls.at(index)->vel.loc); rc.fin_pos.dir=vec2goal.dir().radian(); KeepInField(rc); if(((wm->ourRobot[id].pos.loc-point2).length())>800) state=0; if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0; if(hole1.contains(rc.fin_pos.loc) || hole2.contains(rc.fin_pos.loc)) { // vec2goal.setLength(100); rc.fin_pos.loc=point2 ;//+ vec2goal; // rc.fin_pos.loc=point2; } // reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos.loc,rc.fin_pos.loc,40); // if(reach) // state2 = 0; } break; } int mrgn=200; Vector2D dlta; if(IsInmargins(point2,mrgn)) { int side = ((point2.x-mean_x)/abs(point2.x-mean_x))*((point2.y-mean_y)/abs(point2.y-mean_y)); if(point2.x > MAX_X-mrgn || point2.x < MIN_X+mrgn) { side *= ((point2.y-mean_y)/abs(point2.y-mean_y)); dlta=Vector2D(0,side*(ROBOT_RADIUS+20));} else if(point2.y > MAX_Y-mrgn || point2.y < MIN_Y+mrgn) { side *=((point2.x-mean_x)/abs(point2.x-mean_x)); dlta=Vector2D(side*(ROBOT_RADIUS+20),0);} switch(statemargin) { case 0:{ rc.fin_pos.loc=point2+dlta; int rad = 70+ROBOT_RADIUS; Circle2D c(point2,rad); rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc); qDebug()<< "In Margins Pos : ball = ( " << point2.x << ","<< point2.y << ")"; qDebug()<< "In Margins Pos : delta = ( " << dlta.x << ","<< dlta.y << ")"; qDebug()<< "In Margins Pos : fin_pos = ( " << rc.fin_pos.loc.x << ","<<rc.fin_pos.loc.y << ")"; qDebug()<< "In Margins Pos : Robot = ( " << wm->ourRobot[id].pos.loc.x << ","<<wm->ourRobot[id].pos.loc.y << ")"; rc.fin_pos.dir=dlta.dir().radian()-side*M_PI/2; reach=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,7); // wm->ourRobot[id].pos.loc,rc.fin_pos.loc,200); qDebug() << "dist To final Pos : " << (wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length(); qDebug() << " Avoided : " << Avoided << " reach" << reach; if(reach) statemargin = 1; } break; case 1:{ rc.fin_pos.dir = dlta.dir().radian() - side*0.9*M_PI ; rc.fin_pos.loc=point2-dlta; qDebug() << "Fin_POS . dir = " << AngleDeg::rad2deg(rc.fin_pos.dir) << " ROBOT . dir = " << AngleDeg::rad2deg(wm->ourRobot[id].pos.dir); if(((wm->ourRobot[id].pos.loc-point2).length())>300) statemargin=0; double delta_ang=wm->ourRobot[id].pos.dir-rc.fin_pos.dir; if (delta_ang > M_PI) delta_ang -= (M_PI * 2); if (delta_ang < -M_PI) delta_ang += (M_PI * 2); if(fabs(delta_ang) < AngleDeg::deg2rad(10)) statemargin=0; rc.maxSpeed=1.7; // bool chighz=wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,20,10); // if(chighz) statemargin=0; // if((wm->ourRobot[id].pos.loc.dir()-rc.fin_pos.dir)<AngleDeg::deg2rad(10)) statemargin=0; // if(((wm->ourRobot[id].pos.loc-rc.fin_pos.loc).length())<250) state=0; } break; } } } // if(DontEnterCircle && circularBorderDANGER.contains(wm->ourRobot[id].pos.loc) && circularBorder2.contains(point2))//circularBorderDANGER.contains(rc.fin_pos.loc)) // { // rc.fin_pos.loc=circularBorderDANGER.nearestpoint(point2);//wm->ourRobot[id].pos.loc;//AvoidtoEnterCircle(circularBorderDANGER,point2); // rc.maxSpeed=1.1; // } // qDebug()<< "BEFOR ANY CHANGE " << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2; // qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y; // qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length(); rc.fin_pos.loc=KeepInField(rc); if(/*((wm->ourRobot[id].pos.loc-opp).length() < 700)*/!OppIsKhoraak && oppIsValid ) { rc.fin_pos.loc=AvoidtoEnterCircle(circularBorderDANGER,wm->ourRobot[id].pos.loc,rc.fin_pos.loc);//rc.fin_pos.loc); } rc.fin_pos.loc=AvoidtoEnterCircle(hole1_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc); rc.fin_pos.loc=AvoidtoEnterCircle(hole2_Offset,wm->ourRobot[id].pos.loc,rc.fin_pos.loc); if(oppIsValid && OppIsKhoraak) rc.fin_pos.loc=opp; qDebug() << "State Margin = " << statemargin; // if(!OppIsKhoraak) // { // Circle2D c(opp,2*ROBOT_RADIUS+350); // rc.fin_pos.loc=AvoidtoEnterCircle(c,wm->ourRobot[id].pos.loc,rc.fin_pos.loc); // } // bool rich = wm->kn->ReachedToPos(wm->ourRobot[id].pos,rc.fin_pos,10,5); // if(rich) rc.fin_pos.loc=wm->ourRobot[id].pos.loc; // rc.fin_pos.loc=Vector2D(60000,60000); // rc.fin_pos.loc=KeepInField(rc); // rc.fin_pos.loc=Vector2D(MAX_X,MAX_Y); // qDebug() << " DIRECTION ERROR = " << (rc.fin_pos.dir-wm->ourRobot[id].pos.dir)*(180/3.14); //rc.useNav = true; // rc.maxSpeed=1.8*rc.maxSpeed; // rc.isBallObs = false; // rc.isKickObs = false; qDebug()<< "INDEX = " << index << "fin_pos.x " << rc.fin_pos.loc.x << " Y "<<rc.fin_pos.loc.y<< " ------------------------------ STATE = " << state << " STATE 2 =" << state2; //qDebug()<< " " << "ROBOT POS.x " << wm->ourRobot[id].pos.loc.x << " Y "<< wm->ourRobot[id].pos.loc.y; //qDebug() << "Distance To Fin_Pos = " << (rc.fin_pos.loc-wm->ourRobot[id].pos.loc).length(); // qDebug() << " IS INSIDE = " << IsInside << " UN ACCESSIBLE :" << unAccessible; // qDebug() << " BALL SIZE : " << wm->balls.size(); // qDebug() << " ROBOT IS IN CORNER " << IsInCorner(wm->ourRobot[id].pos.loc,70) << " Robot Is In Margin : " << IsInmargins(wm->ourRobot[id].pos.loc,70) ; rc.maxSpeed=1.21*rc.maxSpeed; return rc; }