//Begin page main /// // ISS code - 22/12/2018 // Both base and champions configurations are automatically considered // #define HIGH_SPEED 1.8f//1.8 #define VERY_LOW_SPEED 1.0f #define LOW_SPEED 1.f//1.2 #define PI_G 3.1415f // // Definition of code variables // bool flagZ; bool champion; short Time, timeStop; short Phase; float dist, kVel, stopAt, chSpeed; float mySpeed,othSpeed; float deltaEulerx,oldEulerx,diffEuler; float tresholdx,tresholdy,tresholdz; float myEulerAngles[3],otherEulerAngles[3]; float Target[3]; ZRState myState; ZRState myEuler; ZRState otherEuler; void init(){ champion=true; flagZ=false; timeStop=200; Phase=100; dist=10.0f; stopAt=0.01f; chSpeed=0.0f; tresholdx=0.0f;//-0.02 tresholdy=0.0f;//-0.04 tresholdz=0.0f;//-0.04 // // champions // kVel=HIGH_SPEED; } void loop(){ float v[3],w[3]; Time=api.getTime(); api.getMyZRState(myState); game.getMyEulerState(myEuler);game.getOtherEulerState(otherEuler); mathVecSubtract(v,otherEuler,myEuler,3); mySpeed=mathVecMagnitude(&myEuler[3],3);othSpeed=mathVecMagnitude(&otherEuler[3],3); // if(Time==0){ oldEulerx=otherEuler[6]; DEBUG(("Hallo ISS from the Approximate Prism Intrnational Park")); } if(Time>3&&Time<10&&(fabs(otherEuler[0])>0.01f||fabs(otherEuler[2])>0.01f))champion=false; // tresholdy=0.0f;//-0.04 if(!flagZ){ if(champion){ tresholdx=0.045f;//0.01 tresholdz=0.05f;//-0.04 }else{ tresholdx=0.04f;//0.035 tresholdz=-0.06f;//-0.06 } } // mVecMultAndNorm (otherEulerAngles,&otherEuler[6],1.0f,false); if(champion)diffEuler=otherEuler[6]-oldEulerx; else diffEuler=0.0f; ///DEBUG(("****DIFFEuler = %f; champion = %i; myspeed = %f\n",diffEuler,champion, mySpeed)); // if(Time>4&&champion)myEulerAngles[0]=-otherEulerAngles[0]+PI_G-diffEuler*2.5f; else myEulerAngles[0]=-otherEulerAngles[0]; myEulerAngles[1]=-otherEulerAngles[1]; myEulerAngles[2]=otherEulerAngles[2]-PI_G; // // Phase=100 - Hooking the target sphere // if(Phase==100){ // if(myEuler[1]<0.0f)kVel=LOW_SPEED; // if(myEuler[1]<0.f&&mySpeed>0.05f)BrakeF(); if(dist<0.10f){ if(champion)kVel=0.012f; else kVel=0.012f; } // hookTheSphere(); // ///DEBUG(("****DISTbetCENTRES = %f; myspeed = %f\n",distBetweenCentres,mySpeed)); if(game.getGamePhase()==4){ chSpeed=10.0f;kVel=4.0f;stopAt=0.001f; Target[0]=1.65f;Target[1]=0.75f;Target[2]=1.65f; if(myEuler[2]>0.f)Target[2]=-1.65f; if(myEuler[0]>0.f)Target[0]=-1.65f; Phase=200; } } // // Returning to the base // if(Phase==200){ // returningHome(); } // game.setEulerTarget(myEulerAngles); oldEulerx=otherEuler[6]; } //End page main //Begin page vGameLibs void hookTheSphere(){ float hookCorrx[3],hookCorry[3],hookCorrz[3],hookCorr[3]; float myCentre[3],otherCentre[3]; float v[3],w[3],y[3]; float distBetwCentres; // // The following instructions evaluate a slight correction ortogonal to hook direction (negative when along z) // hookCorrx[0]=cosf(otherEulerAngles[2])*cosf(otherEulerAngles[1]); hookCorrx[1]=sinf(otherEulerAngles[2])*cosf(otherEulerAngles[1]); hookCorrx[2]=-sinf(otherEulerAngles[1]); // // mVecMultAndNorm (w,&myState[6],0.17095f,false); mVecMultAndNorm (y,hookCorrx,0.17095f,false); mathVecAdd(myCentre,myState,w,3);mathVecAdd(otherCentre,otherEuler,y,3); mathVecSubtract(v,myCentre,otherCentre,3); distBetwCentres=mathVecMagnitude(v,3); // // mVecMultAndNorm (v,hookCorrx,0.3419f,false);//0.3419 mathVecAdd(Target,v,otherEuler,3); hookCorrz[0]=cosf(otherEulerAngles[2])*sinf(otherEulerAngles[1])*cosf(otherEulerAngles[0])+sinf(otherEulerAngles[2])*sinf(otherEulerAngles[0]); hookCorrz[1]=sinf(otherEulerAngles[2])*sinf(otherEulerAngles[1])*cosf(otherEulerAngles[0])-cosf(otherEulerAngles[2])*sinf(otherEulerAngles[0]); hookCorrz[2]=cosf(otherEulerAngles[1])*cosf(otherEulerAngles[0]); // // champions if(champion){ if(myEuler[1]<0.0f&&myCentre[1]<-0.33f&&!flagZ){tresholdx=-0.002;tresholdz=0.004f;timeStop=Time+3;deltaEulerx=0.0f;flagZ=true;}//tresholdx=0.0f; if(Time>23&&!flagZ){tresholdx=-0.03f;tresholdz=0.02f;flagZ=true;timeStop=Time+6;} if(Time>timeStop){tresholdx=0.0f;tresholdz=0.0f;flagZ=true;}//tresholdx=0.0f;DEBUG(("***STOP corrX at time = %i\n",Time)); } else { if(myEuler[1]<0.0f&&myCentre[1]<-0.31f&&!flagZ){tresholdz=0.0f;tresholdx=-0.003f;timeStop=Time+3;flagZ=true;}// if(Time>23&&!flagZ){tresholdx=-0.02f;flagZ=true;tresholdz=0.0f;timeStop=Time+6;} if(Time>timeStop){tresholdx=0.0f;}//tresholdx=0.0f;DEBUG(("***STOP corrX at time = %i\n",Time)); } mVecMultAndNorm(hookCorrx,hookCorrx,tresholdx,true); mVecMultAndNorm(hookCorrz,hookCorrz,tresholdz,true); mathVecAdd(hookCorr,hookCorrx,hookCorrz,3); mathVecAdd(Target,Target,hookCorr,3); // // if(dist<0.03f&&mySpeed>0.008f&&!game.getHookCollision())BrakeF();//0.05 if(kVel<1.f&&flagZ){ if(champion)kVel=0.01f; else kVel=0.012f; } /// /// api.setPosGains(0.15f,1.0f,2.5f); reachNewTarget(stopAt); //DEBUG(("***distCentres = %f, kVel = %f, flagZ =%i\n",distBetwCentres,kVel,flagZ)); //DEBUG(("***TRESHLOLDx = %f, TRESHLOLDz = %f\n",tresholdx,tresholdz)); } // void returningHome(){ float v[3]; mVecMultAndNorm (v,myEuler,1.0f,false);v[1]=v[1]+3.0f; if(otherEuler[1]>0.f){ reachNewTarget(stopAt); } else api.setPositionTarget(v); } //******************************************************************************************* // Kinematic routines //Begin page vKinematics // void mVecMultAndNorm(float ret[3], float vinp[3], float mult, bool norm){ memcpy(ret,vinp,sizeof(float)*3); if(norm) mathVecNormalize(ret,3); for(int i = 0;i <3;i++){ ret[i] *= mult; } } // float distBetweenPoints(float v[3], float w[3]){ float d[3]; mathVecSubtract(d,v,w,3); return mathVecMagnitude(d,3); } // a routine to move quite fast (or saving energy, when k< 0.95) towards the selected target float setPosAndGo(float k, float brakept){ float v[3],dtoT; mathVecSubtract(v,Target,myEuler,3); dtoT=mathVecMagnitude(v,3); // go, but slowly if(k<1.0f){ if(dtoT>brakept){ // if(mySpeed0.005f){ mVecMultAndNorm (v,v,k,true); api.setVelocityTarget(v); } else{ api.setPositionTarget(Target); } return dtoT; } k=1.0f; } // go, go, gooooo.... mVecMultAndNorm (v,v,k,false); mathVecAdd (v,v,myEuler,3); api.setPositionTarget(v); return dtoT; } // void reachNewTarget(float brake){ dist=setPosAndGo(kVel,chSpeed); // ////DEBUG(("Reach_dist = %f; kVel = %f\n", dist, kVel)); // if(dist0.005f)BrakeF(); } return; } // void BrakeF(){ float v[3]; ///DEBUG(("braking!! = %f\n", mySpeed)); mVecMultAndNorm(v,&myEuler[3],-0.2f,true); api.setForces(v); } //End page main