Advertisement
Guest User

Untitled

a guest
Nov 23rd, 2017
377
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 14.70 KB | None | 0 0
  1. //Begin page main
  2. //Begin page defines
  3. #define MAX_ACCEL 0.0088f
  4. #define ATT_PID_P_DEFAULT 0.0045f
  5. #define ATT_PID_I_DEFAULT 0.00018f
  6. #define ATT_PID_D_DEFAULT 0.0143f
  7. #define ATT_PID_P_GAIN (ATT_PID_P_DEFAULT * 1.26f)
  8. #define ATT_PID_I_GAIN (ATT_PID_I_DEFAULT * 1.12f)
  9. #define ATT_PID_D_GAIN (ATT_PID_D_DEFAULT * 1.12f)
  10. #define POS_PD_P_DEFAULT 0.172f
  11. #define POS_PD_D_DEFAULT 1.720f
  12.  
  13. //End page defines
  14. //Begin page main
  15. /**
  16. *This is the first working alliance code that we have
  17. *It finds the highest peak using a spiral search pattern and then drills
  18. *Ignroes all concentration
  19. */
  20. int phase;
  21. int drillCount;
  22.  
  23. float targetDrillPos[3];
  24. float targetQuat[4];
  25.  
  26. bool sampleDropped;
  27. bool geyserSpawned;
  28.  
  29.  
  30. //conc
  31. float lastTargetDrillPos[3];
  32. float lastLastTargetDrillPos[3];
  33. float currentHighestConcPos[3];
  34. float currentHighestConc;
  35.  
  36.  
  37.  
  38.  
  39. int enemyInBaseTime;
  40.  
  41. void init(){
  42. //This function is called once when your code is first loaded.
  43. //IMPORTANT: make sure to set any variables that need an initial value.
  44. //Do not assume variables will be set to 0 automatically!
  45.  
  46. phase=0;
  47. drillCount = 0;
  48. sampleDropped = false;
  49. geyserSpawned = false;
  50.  
  51. currentHighestConc = 0.10f;
  52.  
  53. enemyInBaseTime = 0;
  54. api.setAttGains(ATT_PID_P_GAIN,ATT_PID_I_GAIN,ATT_PID_D_GAIN);
  55.  
  56. }
  57.  
  58. void loop(){
  59.  
  60.  
  61.  
  62. api.setPosGains(0.355f,0.0f,2.1f);
  63. float otherState[12];
  64. float myState[12];
  65. float mySState[13];
  66. api.getMyZRState(myState);
  67. api.getMySphState(mySState);
  68. api.getOtherZRState(otherState);
  69. float zero[3] = {0.0f,0.0f,0.0f};
  70. float negZdir[3] = {0.0f,0.0f,-1.0f};
  71. //closest point on baseStation
  72. float baseStation[3];
  73. getPointBetween(baseStation,targetDrillPos,zero, 0.225f);
  74.  
  75. bool drillError = game.getDrillError();
  76. bool inGeyser = game.isGeyserHere(myState);
  77. int numSamplesHeld = game.getNumSamplesHeld();
  78. float fuelRemaining = game.getFuelRemaining();
  79.  
  80. int time = api.getTime();
  81.  
  82.  
  83. //t0 init
  84. if (time==0){
  85.  
  86. targetQuat[0] = 0.0f;
  87. targetQuat[1] = 0.0f;
  88. targetQuat[2] = 0.0f;
  89. targetQuat[3] = 0.0f;
  90.  
  91. if (myState[0] < 0){
  92. targetQuat[0] = -1.0f;
  93. }else {
  94. targetQuat[1] = -1.0f;
  95. }
  96.  
  97. getLowZDrillPosSpiralSearch(myState,targetDrillPos,0.4f);
  98.  
  99. //initialize
  100. //memcpy(lastLastTargetDrillPos,targetDrillPos,12.0f);
  101. memcpy(lastTargetDrillPos,targetDrillPos,12.0f);
  102. }
  103.  
  104.  
  105.  
  106. DEBUG(("ENEMEY IN BASE TIME %d",enemyInBaseTime));
  107. //enemy base camp detection
  108. if (mathVecMagnitude(otherState,3) < 0.24f){
  109. enemyInBaseTime++;
  110. }else enemyInBaseTime = 0;
  111.  
  112.  
  113. //time break
  114. if ((time > 160 || fuelRemaining <0.17f) && numSamplesHeld >= 2 && enemyInBaseTime < 20){
  115. phase = 2;
  116. game.stopDrill();
  117. }
  118.  
  119.  
  120.  
  121.  
  122.  
  123. switch(phase){
  124. //Case 0 Go to point, set up drill & start drill
  125. case 0:{
  126.  
  127. api.setQuatTarget(targetQuat);
  128.  
  129. if (inGeyser){
  130. //reuse zero save codespace
  131. mathVecSubtract(zero,targetDrillPos,myState,3);
  132. zero[2] = 0.3f;
  133. mathVecScale(zero,zero,2.1f,true);
  134. api.setForces(zero);
  135.  
  136.  
  137.  
  138. }else{
  139. //if (phase0GeyserCtrl) api.setPosGains(0.355f,0.0f,1.45f);
  140. goTo(myState,targetDrillPos);
  141. }
  142.  
  143.  
  144. float currentAtt2D[3] = {myState[6],myState[7],0.0f};
  145. mathVecNormalize(currentAtt2D,3);
  146. float angle = angleBetweenAtt(&myState[6],currentAtt2D);
  147.  
  148. if (distanceBetween(myState,targetDrillPos) < 0.017f
  149. && myState[2] > targetDrillPos[2] - 0.01f /*-radius - max dist 0.04 */
  150. && myState[2] < targetDrillPos[2] + 0.03f /*-radius*/
  151. && angle < 0.19f /*about 11.25deg in radians */
  152. && angle > -0.19f
  153. && mathVecMagnitude(&myState[3],3) < 0.01f
  154. && mathVecMagnitude(&myState[9],3) < 0.04f
  155. ){
  156.  
  157. phase = 1;
  158. game.startDrill();
  159.  
  160. }
  161.  
  162. break;
  163. }
  164. //Case 1 Drill & find new targetDrillPos after drill
  165. case 1:{
  166. geyserSpawned= game.isGeyserHere(targetDrillPos);
  167.  
  168. if (drillCount == 3 || (numSamplesHeld == 5 && sampleDropped) || geyserSpawned || drillError ){
  169.  
  170. drillCount = 0;
  171.  
  172.  
  173. memcpy(lastLastTargetDrillPos,lastTargetDrillPos,12.0f);
  174. memcpy(lastTargetDrillPos,targetDrillPos,12.0f);
  175.  
  176. //search from the highest conc square if we have conc > default -- also allow up to 0.48f depth instead of 0.4f
  177. getLowZDrillPosSpiralSearch((currentHighestConc != 0.1f ? currentHighestConcPos : baseStation),targetDrillPos, (currentHighestConc != 0.1f ? 0.48f : 0.4f));
  178.  
  179.  
  180.  
  181.  
  182.  
  183. phase = 2;
  184. //we don't have samples anyway; just go back to phase 0
  185. if (!inGeyser && numSamplesHeld < 4) phase =0;
  186. if (geyserSpawned && numSamplesHeld < 3 ) {
  187. phase = 0;
  188. // phase0GeyserCtrl = true;
  189. mathVecSubtract(zero,targetDrillPos,myState,3);
  190. zero[2] = 0.3f;
  191. mathVecScale(zero,zero,2.3f,true);
  192. api.setForces(zero);
  193.  
  194. }
  195. if (!drillError && (fuelRemaining < 0.05f || time > 170)) phase = 1; //angular momentum - just keep drilling
  196. else game.stopDrill();
  197.  
  198. }else{
  199.  
  200. //reuse zero
  201. getNextAttOrbitAboutZ(zero,&myState[6], -1000.0f);
  202.  
  203. if (game.checkSample()){
  204.  
  205. drillCount++;
  206. //extra point for additional drill
  207. if (numSamplesHeld == 5){
  208. //DEBUG(("SAMPLE DROP"));
  209. sampleDropped = true;
  210. game.dropSample(4);
  211. }
  212. game.pickupSample();
  213. api.attVec2Quat(&myState[6],zero,&mySState[6],targetQuat);
  214.  
  215. }
  216.  
  217. api.setPositionTarget(targetDrillPos);
  218. api.setAttitudeTarget(zero);
  219. }
  220.  
  221.  
  222. break;
  223. }
  224. //Deposit & analyze sample; go back to last dropPos +1y if the deposit has higher conc
  225. case 2:{
  226.  
  227. //reset extra sample drop bool
  228. sampleDropped = false;
  229.  
  230. if (game.atBaseStation() || enemyInBaseTime >= 20){
  231.  
  232. float highestConcOfDrop;
  233. float lastLastDrillConc = 0.0f;
  234. float lastDrillConc = 0.0f;
  235.  
  236. bool samplesHeld[5];
  237. game.getSamplesHeld(samplesHeld);
  238.  
  239. for (int i=0;i<5;i++){
  240. if (samplesHeld[i]){
  241. //lastLastDrillConc 0 to 2
  242. if (i <= 2) lastLastDrillConc = game.dropSample(i);
  243. //lastDrillConc 3 to 4
  244. else lastDrillConc = game.dropSample(i);
  245. }
  246. }
  247.  
  248.  
  249. highestConcOfDrop = lastDrillConc > lastLastDrillConc ? lastDrillConc : lastLastDrillConc;
  250.  
  251. DEBUG(("lastLastDrillConc %f",lastLastDrillConc));
  252. DEBUG(("lastDrillConc %f",lastDrillConc));
  253. DEBUG(("HIGHEST CONC OF DROP %f",highestConcOfDrop));
  254.  
  255.  
  256.  
  257. if (highestConcOfDrop > currentHighestConc){
  258. DEBUG(("NEW HIGHEST CONC FOUND; CHANGING DRILLTARGETPOS BACK"));
  259. currentHighestConc = highestConcOfDrop; //update currentHighest
  260.  
  261. //record highest conc pos in currentHighestConcPos
  262. //Check lastLast first since its probably closer to the center (if highestConcOfDrop == lastLastDrillConc == lastDrillConc)
  263. memcpy(currentHighestConcPos,(highestConcOfDrop == lastLastDrillConc ? lastLastTargetDrillPos : lastTargetDrillPos),12.0f);
  264. //go out in the +y direction
  265. //spiral search from the highest conc pos
  266. getLowZDrillPosSpiralSearch(currentHighestConcPos,targetDrillPos,0.48f);
  267.  
  268.  
  269.  
  270. }
  271.  
  272.  
  273.  
  274. phase = 0;
  275.  
  276. }
  277.  
  278. api.setAttitudeTarget(negZdir);
  279. api.setAttRateTarget(zero);
  280.  
  281.  
  282. bool geyserCtrl = false;
  283. if (inGeyser ){
  284. geyserCtrl = true;
  285. //move out of geyser
  286. mathVecScale(zero,baseStation,-0.35f,true);
  287. }else if (geyserSpawned && myState[5] < -0.014f ){
  288. geyserCtrl = true;
  289. //"zero" is actually scaledDrillPos; UNETHICAL CODESIZE REDUCTION
  290. mathVecScale(zero,targetDrillPos,2.0f,true);
  291. }
  292.  
  293. if (geyserCtrl) api.setPosGains(POS_PD_P_DEFAULT,0.0f,POS_PD_D_DEFAULT);
  294. //"zero" is actually scaledDrillPos; UNETHICAL CODESIZE REDUCTION
  295. goTo(myState,(geyserCtrl ? zero : baseStation));
  296.  
  297.  
  298.  
  299.  
  300. break;
  301. }
  302.  
  303.  
  304.  
  305. }
  306.  
  307.  
  308. }
  309.  
  310. //End page main
  311. //Begin page zFunc
  312.  
  313. void getPointBetween(float resultant[3],float myState[12],float targetPos[3],float factor) {
  314. //gets the point that is between you and the target pos, but with the distance "factor" away starting from target going to sphere
  315. float vectorBetween[3];
  316. mathVecSubtract(vectorBetween,targetPos,myState,3);
  317. float lengthOfVector = ((distanceBetween(myState,targetPos)-factor));
  318. mathVecScale(vectorBetween,vectorBetween,lengthOfVector,true);
  319. mathVecAdd(resultant,vectorBetween,myState,3);
  320. }
  321.  
  322.  
  323. // Returns the angle between two vectors
  324. float angleBetween(float vector1[3], float vector2[3]) {
  325. return acosf(mathVecInner(vector1, vector2, 3) / (mathVecMagnitude(vector1, 3) * mathVecMagnitude(vector2, 3)));
  326. }
  327.  
  328. float angleBetweenAtt(float att1[3],float att2[3]){
  329. return acosf(mathVecInner(att1,att2,3));
  330.  
  331. }
  332.  
  333.  
  334.  
  335. float distanceBetween(float myState[3], float otherState[3]) {
  336. float diff[3];
  337. mathVecSubtract(diff, myState, otherState,3);
  338. return mathVecMagnitude(diff,3);
  339. }
  340.  
  341.  
  342. void mathVecScale(float res[3], float src[3], float mag, bool norm) {
  343. memcpy(res,src,12.0f);
  344. if(norm) mathVecNormalize(res,3);
  345. for (int i=0;i<3;i++) res[i]*=mag;
  346. }
  347.  
  348.  
  349.  
  350. void pointToward(float myState[12],float target[3]) {
  351.  
  352. mathVecSubtract(target, target, myState, 3);
  353. mathVecNormalize(target, 3);
  354. api.setAttitudeTarget(target);
  355. }
  356.  
  357.  
  358.  
  359. void getNextAttOrbitAboutZ(float finalAtt[3],float myAtt[3],float rotationRate = 1000.0f){
  360. //Z axis
  361. float zVec[3] = {0.0f,0.0f,-1.0f};
  362. float zCrossAtt[3];
  363. //vector to add to our att
  364. float vecToAdd[3];
  365.  
  366. //see the y0botics circle tutorial for more info
  367. mathVecCross(zCrossAtt,myAtt,zVec);
  368. mathVecScale(vecToAdd,zCrossAtt,rotationRate,true);
  369. mathVecAdd(finalAtt,myAtt,vecToAdd,3);
  370. mathVecNormalize(finalAtt,3);
  371. }
  372.  
  373. //brakingAccel MAX a is 0.008476f = 0.00825f
  374. void goTo(float myState[12],float targetPoint[3]/*,float brakingAccel = 0.00825f*/){
  375.  
  376. float distanceToTarget = distanceBetween(myState,targetPoint);
  377.  
  378.  
  379. //@@@@@@@@@@@@@@@currentVel can be made into a GLOBAL VARIABLE BE SCARED
  380. float currentVel = mathVecMagnitude(&myState[3],3);
  381. float targetVel = 0.06f;
  382.  
  383. if (currentVel < 0.05f){
  384. //when at low velocity, accelerate AS FAST AS POSSIBLE
  385. targetVel = 100.00f;
  386. }
  387.  
  388. /**we want to brake as HARD AS POSSIBLE i.e. we want the HIGHEST acceleration
  389. *MAX a is 0.008476f = 0.00825f
  390. *vf^2 = vi^2 + 2ad
  391. *vf is 0
  392. *(0) = (vi^2) + 2ad
  393. *-vi^2 = 2ad
  394. * d=-(vi)^2/2a
  395. *v0=instant v ---> SPHERE Target vf=0
  396. *------------------------------------------------
  397. * a=(-)0.00825f <-
  398. */
  399. //negatives cancel
  400. if (distanceToTarget <= (currentVel*currentVel)/(2*MAX_ACCEL)) targetVel = 0.0f;
  401.  
  402.  
  403.  
  404. if (distanceToTarget>0.32f) {
  405. float targetVelVec[3];
  406. mathVecSubtract(targetVelVec,targetPoint,myState,3);
  407. mathVecScale(targetVelVec,targetVelVec,targetVel,true);
  408. api.setVelocityTarget(targetVelVec);
  409. }
  410. //when you're close enough, revert to setPosTarget
  411. else api.setPositionTarget(targetPoint);
  412.  
  413. }
  414.  
  415. //End page zFunc
  416. //Begin page zNewFunc
  417. void getLowZDrillPosSpiralSearch(float startPos[3],float result[3],float minZ){
  418. int tempS[3];
  419. float currentPos[3];
  420. game.pos2square(startPos,tempS);
  421. game.square2pos(tempS,currentPos);
  422. currentPos[2] = 0.0f;
  423. //kind of like a "bug" that moves in a spiral
  424. float searchOffset[3] = {0.08f,0.0f,0.0f};
  425. int sideLength = 0;
  426. int stepsPerSide = 0;
  427. int totalTurns = 0;
  428.  
  429.  
  430. //search in a "spiral"
  431. //recalculate if the pos is in base station/has been drilled on before
  432.  
  433. while(game.getTerrainHeight(currentPos) > minZ || mathVecMagnitude(currentPos,3) <= 0.24f || game.getDrills(currentPos) > 0){
  434. //'move' the bug
  435. if (stepsPerSide < sideLength){
  436. mathVecAdd(currentPos,currentPos,searchOffset,3);
  437. stepsPerSide++;
  438. }else{
  439. totalTurns++;
  440. //turn our searchOffset vector 90 degrees
  441. float newX = -searchOffset[1];
  442. float newY = searchOffset[0];
  443. searchOffset[0] = newX;
  444. searchOffset[1] = newY;
  445.  
  446. //hit a corner; turn
  447. if (totalTurns % 2 != 0) sideLength++;
  448. stepsPerSide = 0;
  449. }
  450.  
  451. };
  452.  
  453. result[0] = currentPos[0];
  454. result[1] = currentPos[1];
  455. result[2] = game.getTerrainHeight(currentPos) - 0.14f; //subtract radius + 0.03f tolerance
  456.  
  457. }
  458. //End page zNewFunc
  459.  
  460.  
  461. //End page main
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement