Ejemplo n.º 1
0
void ContainerGrabber::initGrabSequence() {
    setControlMode(leftArm, "position");
    setControlMode(rightArm, "position");
    setPIDslot(leftArm, 0);
    setPIDslot(rightArm, 0);
    setPositionTarget(leftArm, dropTargetAngle);
    setPositionTarget(rightArm, dropTargetAngle);
}
Ejemplo n.º 2
0
void loop(){
	api.getMyZRState(me);
	api.getOtherZRState(other);
	
	for(int i = 0; i < 9; i++){
	    if(itemBool[i] && game.hasItem(i) != -1){
	        itemBool[i] = 0;
	    }
	}
	
	switch(state) {
	    case 0:
	        mathVecSubtract(facing,other,me,3);
	        mathVecNormalize(facing,3);
	        api.setAttitudeTarget(facing);
	        //api.setPositionTarget(target);
	        if(game.isCameraOn() && game.posInLight(other) && game.isFacingOther()){
	            DEBUG(("TAKING PICTURES"));
	            game.takePic();
	        }
	        if(game.getMemoryFilled() == 2)
	            state = 1;
	        break;
	        
	    case 1:
	        game.uploadPics();
	            
	        if(game.getMemoryFilled() == 0)
	            state = 0;
	        break;
	   
	   case 2:
	        mathVecSubtract(facing,target,other,3);
	        mathVecNormalize(facing,2);
	        api.setAttitudeTarget(facing);
	        
	        setPositionTarget(target,3);
	        
	        if(distance(me,target) < 0.1){
	            setPositionTarget(target,8);
	        }
	        
	        if(distance(me,target) < 0.03){
	            state = 0;
	            target[0] = 0.0;
	            target[1] = 0.6;
	            target[2] = 0.0;
	        }
	        break;
	}
}
void rebootIfStorm() {
    
    if (api.getTime() == (solarFlareBegin - 1)) {
	    DEBUG(("I shall now reboot.\n"));
        setPositionTarget(me, 20); //prevents crashing during reboot
		game.turnOff();
		game.turnOn();
		state = 0;
	}
	
	else if (api.getTime() >= (solarFlareBegin) && api.getTime() <= (solarFlareBegin + 2)) {
	    DEBUG(("Ah shit, it's a flare!\n"));
	}
	
	else if (game.getNextFlare() != -1) {
	    solarFlareBegin = api.getTime() + game.getNextFlare();
	    DEBUG(("Next solar flare will occur at %ds.\n", solarFlareBegin));
	}
	
	else {
	    DEBUG(("I don't know when the next flare is.\n"));
	    solarFlareBegin = 1000;
	}
	
}
void setPositionTarget(float target[3], float multiplier) {
    float temp[3];
    
    mathVecSubtract(temp,target,me,3);
    for (int i = 0; i < 3; i++) temp[i] = me[i] + temp[i] * multiplier;
    setPositionTarget(temp);
    DEBUG(("Hauling ass! (*%.1f)\n", multiplier));
}
void loop() {
	int i; // COUNTER!!!!

	api.getMyZRState(myState);
	DEBUG(("\n%d\n",state));

	for (i = 0; i < 3; i++) {game.getPOILoc(POI[i], i);}
	time++;
	nextFlare = game.getNextFlare();
	memoryFilled = game.getMemoryFilled();
	percentFuelRemaining = game.getFuelRemaining() * 100 / 90.0;

	if (time % 60 == 0 && state == TakePic_One) {
		state = Chose_POI;
	}

	// FLARE EVASION

	nextFlare = game.getNextFlare();

	if (nextFlare == -1) {
		DEBUG(("\nI don't know when the next flare is, so stop asking.\n"));
	}
	else if (nextFlare <= 30) {
		if (lastState != GO_TO_SHADOW) {
			lastState = state;
		}
		else {
			lastState = Chose_POI;
		}

		state = GO_TO_SHADOW;
		DEBUG(("\nnextFlare: %d\nOH F**K IT'S A FLARE!!!!!\n",nextFlare));
	}

	switch (state) {
		case Chose_POI:
			{
			float currentMinDist = distance(POI[0],myState);
			bestPOI = 0;
			for (i = 1 ; i < 3 ; i++)  {
				if (distance(POI[i],myState) < currentMinDist) {
					currentMinDist = distance(POI[1],myState);
					bestPOI = i;
				}
				if (POI[i][1] == 0) {
					middlePOI = i;
				}
			}

			memcpy(POILoc, POI[bestPOI], 3*sizeof(float));

			memcpy(brakingPt, POILoc, 3*sizeof(float));

			for (i = 0 ; i < 3 ; i++) {
				brakingPt[i] = 0.5 * brakingPt[i] / mathVecMagnitude(brakingPt,3);
			}

			mathVecRotateToOptimal(brakingPt);
			setPositionTarget(brakingPt,1);
			mathVecSubtract(facing,origin,myState,3);
			api.setAttitudeTarget(facing);

			state = TakePic_One;

			break;
			}

		case TakePic_One:
			{
			if(memoryFilled != 0) {
				memcpy(POILoc, POI[middlePOI], 3*sizeof(float));
				memcpy(brakingPt, POILoc, 3*sizeof(float));

				for (i = 0 ; i < 3 ; i++) {
					brakingPt[i] = 0.5 * brakingPt[i] / mathVecMagnitude(brakingPt,3);
				}

				mathVecRotateToOptimal(brakingPt);

				setPositionTarget(brakingPt,1);
				mathVecSubtract(facing,origin,myState,3);
				api.setAttitudeTarget(facing);

				state = TakePic_Two;
			}

			setPositionTarget(brakingPt,2);
			mathVecSubtract(facing,origin,myState,3);
			api.setAttitudeTarget(facing);
			if (game.alignLine(bestPOI)) {
				game.takePic(bestPOI);
				DEBUG(("\nI AM THE CHAMPION MY FRIENDS!\n"));
			}

			break;
			}

		case TakePic_Two:
			{
			if(memoryFilled == 2) {
				state = GO_TO_SHADOW;
			}
			
			setPositionTarget(brakingPt,2);
			mathVecSubtract(facing,origin,myState,3);
			api.setAttitudeTarget(facing);

			if (game.alignLine(middlePOI)) {
				game.takePic(middlePOI);
				DEBUG(("I AM THE CHAMPION MY FRIENDS!"));
			}

			break;
			}
			

		case GO_TO_SHADOW:
			//DEBUG(("IMPLEMENT LATER"));
			{

			setPositionTarget(uploadPos,1);
			mathVecSubtract(facing,earth,myState,3);
			api.setAttitudeTarget(facing);
			game.uploadPic();
			if (memoryFilled == 0) {
				DEBUG(("WE'LL JUST STAY HERE"));
				if (percentFuelRemaining <= 7) {
					lastState = GO_TO_SHADOW; // 5% fuel is not going to get you anywhere
				}
				state = lastState;
			}
			break;
			}
	}
}
void loop() {
    
    api.getMyZRState(me);

	if(api.getTime() % 60 == 0 && state != 4) state = 0;
	
	DEBUG(("State = %d\n", state));

	picNum = game.getMemoryFilled();

	// Solar Storm Evasion

	if (api.getTime() == (solarFlareBegin - 1)) {
	    DEBUG(("I shall now reboot.\n"));
		game.turnOff();
		game.turnOn();
		state = 0;
	}
	else if (api.getTime() >= (solarFlareBegin) && api.getTime() <= (solarFlareBegin + 2)) {
	    DEBUG(("Ah shit, it's a flare!\n"));
	}
	else if (game.getNextFlare() != -1) {
	    solarFlareBegin = api.getTime() + game.getNextFlare();
	    DEBUG(("Next solar flare will occur at %ds.\n", solarFlareBegin));
	}
	else {
	    DEBUG(("I don't know when the next flare is, so stop asking.\n"));
	    solarFlareBegin = 1000; // fixes a glitch that makes game.getNextFlare()
	    //return 30s at some random point in the beginning of the game,
	    //and from then on returns -1 until the next actual flare, so that the 
	    //SPHERE reboots for no reason
	}
	
	switch (state) {

		case 0: // POI Selection
			game.getPOILoc(POI,0);
			game.getPOILoc(otherPOI,1);
			if (distance(me,otherPOI) <= distance(me,POI)) {
				POIID = 1;
				for (int i = 0; i < 3; i++) POI[i] = otherPOI[i];
			}
			else POIID = 0;
			
			DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));

			for (int i = 0; i < 3; i++) {
				brakingPos[i] = POI[i] * 2.0;
			}
			
			state = 1;
			break;

		case 1: // Orbit Function here
			if (velocity(me) < 0.001) state = 2;
			else {
				setPositionTarget(brakingPos);
			    mathVecSubtract(facing,POI,me,3);
			    mathVecNormalize(facing,3);
			    api.setAttitudeTarget(facing);
			}
			break;

		case 2: // Legacy Code to me
			setPositionTarget(brakingPos);
			mathVecSubtract(facing,POI,me,3);
			mathVecNormalize(facing,3);
			api.setAttitudeTarget(facing);
            game.takePic(POIID);

			if (picNum > 0) {
				DEBUG(("%d picture(s) have been taken\n", picNum));
	            for (int i = 0; i < 3; i++) {
	                uploadPos[i] = me[i] / mathVecMagnitude(me, 3) * 0.6;
	            }
				setPositionTarget(uploadPos);
				state = 3;
			}
			break;

		case 3: // Taking pic in outer zone
			for (int i = 0; i < 3; i++) {
				brakingPos[i] = POI[i] * 2.5;
			}
			
			if (picNum > 1) {
				DEBUG(("%d picture(s) have been taken.\n", picNum));
				for (int i = 0; i < 3; i++) {
	                uploadPos[i] = me[i] / mathVecMagnitude(me, 3) * 0.6;
	            }
				setPositionTarget(uploadPos);
				state = 4;
			}
			
			else {
				api.setPositionTarget(brakingPos);
			    mathVecSubtract(facing,POI,me,3);
			    mathVecNormalize(facing,3);
			    api.setAttitudeTarget(facing);
                game.takePic(POIID);
			}

		case 4: // Upload the picture
			if (velocity(me) < 0.01 && distance(me,uploadPos) < 0.05) {
				game.uploadPic();
				DEBUG(("I just uploaded %d picture(s).\n", picNum));
				DEBUG(("I am in state %d.\n", state)); //Why the f**k does it say it's in State 3???
				state = 0;
			}
			else {
                mathVecSubtract(facing,POI,me,3);
				mathVecNormalize(facing,3);
				api.setAttitudeTarget(facing);

				api.setPositionTarget(uploadPos);
				game.takePic(POIID);
			}

			break;
			
	}
	
	
    if (api.getTime() >= (solarFlareBegin - 3)) {
        setPositionTarget(me);
        DEBUG(("Screw the mission; I'm staying here!\n"));
    } // prevents crashing during reboot
    
}
void loop() {
	
	api.getMyZRState(me);

	if(api.getTime() % 60 == 0 && state != 3) {
	    state = 0;
	    for (int i = 0; i < 3; i++) oldPOIIDs[i] = false;
	}

	rebootIfStorm();
	
	DEBUG(("I am in state %d.\n", state));

	DEBUG(("I can still take %d photos.\n", game.getMemorySize() - game.getMemoryFilled()));
	
	if (state == 0) {// POI Selection
	
		game.getPOILoc(POI,0);
		game.getPOILoc(otherPOI1,1);
		game.getPOILoc(otherPOI2,2);
		
		if (!oldPOIIDs[0] || !oldPOIIDs[1] || !oldPOIIDs[2]) {
		    //if you haven't already been to all the POIs
		    if (oldPOIIDs[0]) for (int i = 0; i < 3; i++) POI[i] = 1000;
		    if (oldPOIIDs[1]) for (int i = 0; i < 3; i++) otherPOI1[i] = 1000;
		    if (oldPOIIDs[2]) for (int i = 0; i < 3; i++) otherPOI2[i] = 1000;
		    //make sure you don't go to a previously-visited POI
		}
		
		if (distance(me,otherPOI1) <= distance(me,POI)) {
			if (distance(me,otherPOI2) <= distance(me,otherPOI1)){
				POIID = 2;
				for (int i = 0; i < 3; i++) POI[i] = otherPOI2[i];
			}
			else {
				POIID = 1;
				for (int i = 0; i < 3; i++) POI[i] = otherPOI1[i];
			}
		}
		
		else if (distance(me,otherPOI2) <= distance(me,POI)) {
			POIID = 2;
			for (int i = 0; i < 3; i++) POI[i] = otherPOI2[i];
		}
		
		else POIID = 0;
		
		DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
	
		state = 1;
		
	}

	if (state == 1) {// Take pic in outer zone
	
		mathVecSubtract(facing,POI,me,3);
		    
    	if (mathVecMagnitude(me,3) > 0.42 && mathVecMagnitude(me,3) < 0.53 && angleBetween(me, facing) > 2.94159265359) {
    	    // in the outer zone and angle between me and POI is less than 0.2
	        picNum = game.getMemoryFilled();
		    game.takePic(POIID);
		    if (game.getMemoryFilled() - picNum == 1) {
	           DEBUG(("I just took a picture in the outer zone.\n"));
	           if (solarFlareBegin - api.getTime() > 13) state = 2;
	           else state = 3;
	        }
		}
		    
		else {
		    for (int i = 0; i < 3; i++) {
		        destination[i] = POI[i] * 0.43 / mathVecMagnitude(POI, 3);
		    }
            if (mathVecMagnitude(facing,3) > 0.5) setPositionTarget(destination, 2);
            else setPositionTarget(destination);
		}
		
		mathVecNormalize(facing,3);
        api.setAttitudeTarget(facing);
    	
	}

	if (state == 2) {// Take pic in inner zone
	
	    mathVecSubtract(facing,POI,me,3);
	
        if (mathVecMagnitude(me,3) < 0.42 && angleBetween(me, facing) > 2.74159265359) {
            //in the inner zone and angle between me and POI is less than 0.4
	        picNum = game.getMemoryFilled();
		    game.takePic(POIID);
		    if (game.getMemoryFilled() - picNum == 1) {
		        DEBUG(("I just took a picture in the inner zone.\n"));
		        state = 3;
		    }
        }
        
        else {
    	    for (int i = 0; i < 3; i++) {
    	        destination[i] = POI[i] * 0.33 / mathVecMagnitude(POI, 3);
    	    }
            if (mathVecMagnitude(facing,3) > 0.5) setPositionTarget(destination, 2);
            else setPositionTarget(destination);
    	}
    	
    	mathVecNormalize(facing,3);
        api.setAttitudeTarget(facing);
		
	}

	if (state == 3) {// Upload the picture
	
		if (mathVecMagnitude(me,3) > 0.53) {
		    picNum = game.getMemoryFilled();
			game.uploadPic();
			DEBUG(("I just uploaded %d picture(s).\n", (picNum - game.getMemoryFilled())));
			state = 0;
			oldPOIIDs[POIID] = true;
		}
	
		else {
			for (int i = 0; i < 3; i++) {
	        	destination[i] = me[i] * 0.61 / mathVecMagnitude(me,3);
	        }
	        if (solarFlareBegin - api.getTime() < 8) setPositionTarget(destination, 4);
            else if (mathVecMagnitude(me,3) < 0.5) setPositionTarget(destination, 2);
            else setPositionTarget(destination);
		}

	}
	
}