Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //Begin page main
- //Begin page defines
- #define MAX_ACCEL 0.0088f
- #define ATT_PID_P_DEFAULT 0.0045f
- #define ATT_PID_I_DEFAULT 0.00018f
- #define ATT_PID_D_DEFAULT 0.0143f
- #define ATT_PID_P_GAIN (ATT_PID_P_DEFAULT * 1.26f)
- #define ATT_PID_I_GAIN (ATT_PID_I_DEFAULT * 1.12f)
- #define ATT_PID_D_GAIN (ATT_PID_D_DEFAULT * 1.12f)
- #define POS_PD_P_DEFAULT 0.172f
- #define POS_PD_D_DEFAULT 1.720f
- //End page defines
- //Begin page main
- /**
- *This is the first working alliance code that we have
- *It finds the highest peak using a spiral search pattern and then drills
- *Ignroes all concentration
- */
- int phase;
- int drillCount;
- float targetDrillPos[3];
- float targetQuat[4];
- bool sampleDropped;
- bool geyserSpawned;
- //conc
- float lastTargetDrillPos[3];
- float lastLastTargetDrillPos[3];
- float currentHighestConcPos[3];
- float currentHighestConc;
- int enemyInBaseTime;
- void init(){
- //This function is called once when your code is first loaded.
- //IMPORTANT: make sure to set any variables that need an initial value.
- //Do not assume variables will be set to 0 automatically!
- phase=0;
- drillCount = 0;
- sampleDropped = false;
- geyserSpawned = false;
- currentHighestConc = 0.10f;
- enemyInBaseTime = 0;
- api.setAttGains(ATT_PID_P_GAIN,ATT_PID_I_GAIN,ATT_PID_D_GAIN);
- }
- void loop(){
- api.setPosGains(0.355f,0.0f,2.1f);
- float otherState[12];
- float myState[12];
- float mySState[13];
- api.getMyZRState(myState);
- api.getMySphState(mySState);
- api.getOtherZRState(otherState);
- float zero[3] = {0.0f,0.0f,0.0f};
- float negZdir[3] = {0.0f,0.0f,-1.0f};
- //closest point on baseStation
- float baseStation[3];
- getPointBetween(baseStation,targetDrillPos,zero, 0.225f);
- bool drillError = game.getDrillError();
- bool inGeyser = game.isGeyserHere(myState);
- int numSamplesHeld = game.getNumSamplesHeld();
- float fuelRemaining = game.getFuelRemaining();
- int time = api.getTime();
- //t0 init
- if (time==0){
- targetQuat[0] = 0.0f;
- targetQuat[1] = 0.0f;
- targetQuat[2] = 0.0f;
- targetQuat[3] = 0.0f;
- if (myState[0] < 0){
- targetQuat[0] = -1.0f;
- }else {
- targetQuat[1] = -1.0f;
- }
- getLowZDrillPosSpiralSearch(myState,targetDrillPos,0.4f);
- //initialize
- //memcpy(lastLastTargetDrillPos,targetDrillPos,12.0f);
- memcpy(lastTargetDrillPos,targetDrillPos,12.0f);
- }
- DEBUG(("ENEMEY IN BASE TIME %d",enemyInBaseTime));
- //enemy base camp detection
- if (mathVecMagnitude(otherState,3) < 0.24f){
- enemyInBaseTime++;
- }else enemyInBaseTime = 0;
- //time break
- if ((time > 160 || fuelRemaining <0.17f) && numSamplesHeld >= 2 && enemyInBaseTime < 20){
- phase = 2;
- game.stopDrill();
- }
- switch(phase){
- //Case 0 Go to point, set up drill & start drill
- case 0:{
- api.setQuatTarget(targetQuat);
- if (inGeyser){
- //reuse zero save codespace
- mathVecSubtract(zero,targetDrillPos,myState,3);
- zero[2] = 0.3f;
- mathVecScale(zero,zero,2.1f,true);
- api.setForces(zero);
- }else{
- //if (phase0GeyserCtrl) api.setPosGains(0.355f,0.0f,1.45f);
- goTo(myState,targetDrillPos);
- }
- float currentAtt2D[3] = {myState[6],myState[7],0.0f};
- mathVecNormalize(currentAtt2D,3);
- float angle = angleBetweenAtt(&myState[6],currentAtt2D);
- if (distanceBetween(myState,targetDrillPos) < 0.017f
- && myState[2] > targetDrillPos[2] - 0.01f /*-radius - max dist 0.04 */
- && myState[2] < targetDrillPos[2] + 0.03f /*-radius*/
- && angle < 0.19f /*about 11.25deg in radians */
- && angle > -0.19f
- && mathVecMagnitude(&myState[3],3) < 0.01f
- && mathVecMagnitude(&myState[9],3) < 0.04f
- ){
- phase = 1;
- game.startDrill();
- }
- break;
- }
- //Case 1 Drill & find new targetDrillPos after drill
- case 1:{
- geyserSpawned= game.isGeyserHere(targetDrillPos);
- if (drillCount == 3 || (numSamplesHeld == 5 && sampleDropped) || geyserSpawned || drillError ){
- drillCount = 0;
- memcpy(lastLastTargetDrillPos,lastTargetDrillPos,12.0f);
- memcpy(lastTargetDrillPos,targetDrillPos,12.0f);
- //search from the highest conc square if we have conc > default -- also allow up to 0.48f depth instead of 0.4f
- getLowZDrillPosSpiralSearch((currentHighestConc != 0.1f ? currentHighestConcPos : baseStation),targetDrillPos, (currentHighestConc != 0.1f ? 0.48f : 0.4f));
- phase = 2;
- //we don't have samples anyway; just go back to phase 0
- if (!inGeyser && numSamplesHeld < 4) phase =0;
- if (geyserSpawned && numSamplesHeld < 3 ) {
- phase = 0;
- // phase0GeyserCtrl = true;
- mathVecSubtract(zero,targetDrillPos,myState,3);
- zero[2] = 0.3f;
- mathVecScale(zero,zero,2.3f,true);
- api.setForces(zero);
- }
- if (!drillError && (fuelRemaining < 0.05f || time > 170)) phase = 1; //angular momentum - just keep drilling
- else game.stopDrill();
- }else{
- //reuse zero
- getNextAttOrbitAboutZ(zero,&myState[6], -1000.0f);
- if (game.checkSample()){
- drillCount++;
- //extra point for additional drill
- if (numSamplesHeld == 5){
- //DEBUG(("SAMPLE DROP"));
- sampleDropped = true;
- game.dropSample(4);
- }
- game.pickupSample();
- api.attVec2Quat(&myState[6],zero,&mySState[6],targetQuat);
- }
- api.setPositionTarget(targetDrillPos);
- api.setAttitudeTarget(zero);
- }
- break;
- }
- //Deposit & analyze sample; go back to last dropPos +1y if the deposit has higher conc
- case 2:{
- //reset extra sample drop bool
- sampleDropped = false;
- if (game.atBaseStation() || enemyInBaseTime >= 20){
- float highestConcOfDrop;
- float lastLastDrillConc = 0.0f;
- float lastDrillConc = 0.0f;
- bool samplesHeld[5];
- game.getSamplesHeld(samplesHeld);
- for (int i=0;i<5;i++){
- if (samplesHeld[i]){
- //lastLastDrillConc 0 to 2
- if (i <= 2) lastLastDrillConc = game.dropSample(i);
- //lastDrillConc 3 to 4
- else lastDrillConc = game.dropSample(i);
- }
- }
- highestConcOfDrop = lastDrillConc > lastLastDrillConc ? lastDrillConc : lastLastDrillConc;
- DEBUG(("lastLastDrillConc %f",lastLastDrillConc));
- DEBUG(("lastDrillConc %f",lastDrillConc));
- DEBUG(("HIGHEST CONC OF DROP %f",highestConcOfDrop));
- if (highestConcOfDrop > currentHighestConc){
- DEBUG(("NEW HIGHEST CONC FOUND; CHANGING DRILLTARGETPOS BACK"));
- currentHighestConc = highestConcOfDrop; //update currentHighest
- //record highest conc pos in currentHighestConcPos
- //Check lastLast first since its probably closer to the center (if highestConcOfDrop == lastLastDrillConc == lastDrillConc)
- memcpy(currentHighestConcPos,(highestConcOfDrop == lastLastDrillConc ? lastLastTargetDrillPos : lastTargetDrillPos),12.0f);
- //go out in the +y direction
- //spiral search from the highest conc pos
- getLowZDrillPosSpiralSearch(currentHighestConcPos,targetDrillPos,0.48f);
- }
- phase = 0;
- }
- api.setAttitudeTarget(negZdir);
- api.setAttRateTarget(zero);
- bool geyserCtrl = false;
- if (inGeyser ){
- geyserCtrl = true;
- //move out of geyser
- mathVecScale(zero,baseStation,-0.35f,true);
- }else if (geyserSpawned && myState[5] < -0.014f ){
- geyserCtrl = true;
- //"zero" is actually scaledDrillPos; UNETHICAL CODESIZE REDUCTION
- mathVecScale(zero,targetDrillPos,2.0f,true);
- }
- if (geyserCtrl) api.setPosGains(POS_PD_P_DEFAULT,0.0f,POS_PD_D_DEFAULT);
- //"zero" is actually scaledDrillPos; UNETHICAL CODESIZE REDUCTION
- goTo(myState,(geyserCtrl ? zero : baseStation));
- break;
- }
- }
- }
- //End page main
- //Begin page zFunc
- void getPointBetween(float resultant[3],float myState[12],float targetPos[3],float factor) {
- //gets the point that is between you and the target pos, but with the distance "factor" away starting from target going to sphere
- float vectorBetween[3];
- mathVecSubtract(vectorBetween,targetPos,myState,3);
- float lengthOfVector = ((distanceBetween(myState,targetPos)-factor));
- mathVecScale(vectorBetween,vectorBetween,lengthOfVector,true);
- mathVecAdd(resultant,vectorBetween,myState,3);
- }
- // Returns the angle between two vectors
- float angleBetween(float vector1[3], float vector2[3]) {
- return acosf(mathVecInner(vector1, vector2, 3) / (mathVecMagnitude(vector1, 3) * mathVecMagnitude(vector2, 3)));
- }
- float angleBetweenAtt(float att1[3],float att2[3]){
- return acosf(mathVecInner(att1,att2,3));
- }
- float distanceBetween(float myState[3], float otherState[3]) {
- float diff[3];
- mathVecSubtract(diff, myState, otherState,3);
- return mathVecMagnitude(diff,3);
- }
- void mathVecScale(float res[3], float src[3], float mag, bool norm) {
- memcpy(res,src,12.0f);
- if(norm) mathVecNormalize(res,3);
- for (int i=0;i<3;i++) res[i]*=mag;
- }
- void pointToward(float myState[12],float target[3]) {
- mathVecSubtract(target, target, myState, 3);
- mathVecNormalize(target, 3);
- api.setAttitudeTarget(target);
- }
- void getNextAttOrbitAboutZ(float finalAtt[3],float myAtt[3],float rotationRate = 1000.0f){
- //Z axis
- float zVec[3] = {0.0f,0.0f,-1.0f};
- float zCrossAtt[3];
- //vector to add to our att
- float vecToAdd[3];
- //see the y0botics circle tutorial for more info
- mathVecCross(zCrossAtt,myAtt,zVec);
- mathVecScale(vecToAdd,zCrossAtt,rotationRate,true);
- mathVecAdd(finalAtt,myAtt,vecToAdd,3);
- mathVecNormalize(finalAtt,3);
- }
- //brakingAccel MAX a is 0.008476f = 0.00825f
- void goTo(float myState[12],float targetPoint[3]/*,float brakingAccel = 0.00825f*/){
- float distanceToTarget = distanceBetween(myState,targetPoint);
- //@@@@@@@@@@@@@@@currentVel can be made into a GLOBAL VARIABLE BE SCARED
- float currentVel = mathVecMagnitude(&myState[3],3);
- float targetVel = 0.06f;
- if (currentVel < 0.05f){
- //when at low velocity, accelerate AS FAST AS POSSIBLE
- targetVel = 100.00f;
- }
- /**we want to brake as HARD AS POSSIBLE i.e. we want the HIGHEST acceleration
- *MAX a is 0.008476f = 0.00825f
- *vf^2 = vi^2 + 2ad
- *vf is 0
- *(0) = (vi^2) + 2ad
- *-vi^2 = 2ad
- * d=-(vi)^2/2a
- *v0=instant v ---> SPHERE Target vf=0
- *------------------------------------------------
- * a=(-)0.00825f <-
- */
- //negatives cancel
- if (distanceToTarget <= (currentVel*currentVel)/(2*MAX_ACCEL)) targetVel = 0.0f;
- if (distanceToTarget>0.32f) {
- float targetVelVec[3];
- mathVecSubtract(targetVelVec,targetPoint,myState,3);
- mathVecScale(targetVelVec,targetVelVec,targetVel,true);
- api.setVelocityTarget(targetVelVec);
- }
- //when you're close enough, revert to setPosTarget
- else api.setPositionTarget(targetPoint);
- }
- //End page zFunc
- //Begin page zNewFunc
- void getLowZDrillPosSpiralSearch(float startPos[3],float result[3],float minZ){
- int tempS[3];
- float currentPos[3];
- game.pos2square(startPos,tempS);
- game.square2pos(tempS,currentPos);
- currentPos[2] = 0.0f;
- //kind of like a "bug" that moves in a spiral
- float searchOffset[3] = {0.08f,0.0f,0.0f};
- int sideLength = 0;
- int stepsPerSide = 0;
- int totalTurns = 0;
- //search in a "spiral"
- //recalculate if the pos is in base station/has been drilled on before
- while(game.getTerrainHeight(currentPos) > minZ || mathVecMagnitude(currentPos,3) <= 0.24f || game.getDrills(currentPos) > 0){
- //'move' the bug
- if (stepsPerSide < sideLength){
- mathVecAdd(currentPos,currentPos,searchOffset,3);
- stepsPerSide++;
- }else{
- totalTurns++;
- //turn our searchOffset vector 90 degrees
- float newX = -searchOffset[1];
- float newY = searchOffset[0];
- searchOffset[0] = newX;
- searchOffset[1] = newY;
- //hit a corner; turn
- if (totalTurns % 2 != 0) sideLength++;
- stepsPerSide = 0;
- }
- };
- result[0] = currentPos[0];
- result[1] = currentPos[1];
- result[2] = game.getTerrainHeight(currentPos) - 0.14f; //subtract radius + 0.03f tolerance
- }
- //End page zNewFunc
- //End page main
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement