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