void toTarget(){
    if(wp){
        if(distance(myState,target)>0.06){
            haulAssTowardTarget(target,1.2);
        }
        else{
            api.setPositionTarget(target);
        }
    }
    else{
        if(distance(myState,target)>0.08){
            haulAssTowardTarget(target,4);
        }
        else{
            api.setPositionTarget(target);
        }
    }
}
コード例 #2
0
void goToTarget(float target[]){
    api.getMyZRState(me);
    switch (state){
        case 0: //initializing
    	    if(pathToTarget(target,waypoint)){
    	        float temp[3];
	            while(pathToTarget(waypoint, temp)){
	                for(int i = 0; i < 3; i++){
	                        waypoint[i] = waypoint[i] + 0.05;
	                }
	            }
    	        mathVecSubtract(originalVecBetween, waypoint, me, 3);
    	        state = 1;
    	    }
    	    else{
    	        state = 2;
    	    }
    	    
    	    break;
	   
	
	    case 1:
	        mathVecSubtract(vecBetween, waypoint, me, 3);
	        float temp[3];
	        if(pathToTarget(target,temp)){
	            api.setVelocityTarget(originalVecBetween); 
	                                   //magnitude stays the same as beginning magnitude
	            DEBUG(("Going to waypoint"));
	            DEBUG(("%f",mathVecMagnitude(originalVecBetween,3)));
	        }
	        else{
	            DEBUG(("going to target"));
	            api.setPositionTarget(target);
	            state = 2;
	        }
	        break;
	    
	    case 2:
	        if(distance(me,target)>0.08){
	            haulAssTowardTarget(target,2);
	            DEBUG(("Going to Target haulAss"));
	        }
	        else{
	            api.setPositionTarget(target);
	        }
	        break;
	}
}
コード例 #3
0
ファイル: Kodak2D.c プロジェクト: StuyNaughtsZR/SpySPHERES
void setPositionTarget(float target[3], float multiplier) {
	api.getMyZRState(me);
	
	float myPos[3],meMag;
	
	memcpy(myPos, me, 3*sizeof(float));
	
	meMag = mathVecMagnitude(myPos,3);
	
	if (minDistanceFromOrigin(target) > 0.32) {
		//if (distance(me, target) < 0.6) { // Save braking distance
		if(distance(me,target)<0.15){
		    haulAssTowardTarget(target,2.5);
		}
		else if(distance(me,target)<0.2){
		    haulAssTowardTarget(target,1.8);
		}
		haulAssTowardTarget(target,1.3);
		//}

		//else { // Or haul ass towards target
		//	float temp[3];

		//	mathVecSubtract(temp,target,me,3);
			
		//	for (int i = 0 ; i < 3 ; i++) {
		//		temp[i] = me[i] + temp[i] * multiplier;
		//	}

		//	api.setPositionTarget(temp);
		//}

		DEBUG(("GOING STRAIGHT\n"));
	}
	
	else if (meMag >= 0.22 && meMag <= 0.315) {
		for (int i = 0; i < 3; i++) {
			myPos[i] = myPos[i] * 2;
		}
		
		api.setPositionTarget(myPos);
		DEBUG(("TOO CLOSE\n"));
	}
	
	else {
		float opposite[3], perpendicular[3], mePrep[3], path[3], temp[3];
		
		mathVecProject(opposite,target,myPos,3);
		mathVecSubtract(perpendicular,target,opposite,3);
		
		for (int i = 0; i < 3; i++) {
		    mePrep[i] = perpendicular[i] / mathVecMagnitude(perpendicular,3);
		}
		
		for (int i = 0; i < 3; i++) {
			mePrep[i] = (mePrep[i] * 0.325 * meMag) / (sqrtf(meMag*meMag - 0.315*0.315));
		}
		
		mathVecSubtract(path,mePrep,myPos,3);
		
		for (int i = 0; i < 3; i++) {
			path[i] = path[i] * multiplier;
		}
		
		mathVecAdd(temp,myPos,path,3);

		api.setPositionTarget(temp);
		
		DEBUG(("TAKING THE TANGENT\n"));
	}
}
void loop() {
    api.getMyZRState(myState);
    time = api.getTime();
    picNum = game.getMemoryFilled();   
    DEBUG(("%d picture(s) have been taken\n", picNum));
    DEBUG(("STATE = %d\n",state));
    DEBUG(("ARRAY = %d,%d,%d\n",goodPOI[0],goodPOI[1],goodPOI[2]));
    DEBUG(("TARGET = %f,%f,%f\n",target[0],target[1],target[2]));
    if(takePic > -1){ //takePic is used to make sure you take an accurate picture.
        takePic--;
    }
    if(time%60 == 0){
        for(int i = 0; i < 3; i++){
            goodPOI[i] = 1;
        }
        state = ST_GET_POI;
    }
    if((time > solarFlareBegin - 25)&&(time < solarFlareBegin)){
        if((state < ST_SHADOW)||(state > ST_SHADOW + 9)){
            state = ST_SHADOW; //if not in shadow state go there.
        }
    }
    else if(time == solarFlareBegin + 3){
        state = ST_GET_POI + 1;
    }
    else if (game.getNextFlare() != -1) {
	    solarFlareBegin = api.getTime() + game.getNextFlare();
	    DEBUG(("Next solar flare will occur at %ds.\n", solarFlareBegin));
	}
    switch(state){
        case ST_GET_POI: //POI selection
            closestPOI(goodPOI,POI,23);
            getPOILoc(POIproj, POIID, 23);
            takePic = 23; //25 probably needs to be changed. only good for the first cycle.
            DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
            state = ST_OUTER;
            break;
            
        case ST_GET_POI + 1: //if coming from shadow zone
            closestPOI(goodPOI,POI,30);
            getPOILoc(POIproj, POIID, 30);
            takePic = 30; //25 probably needs to be changed. only good for the first cycle.
            DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
            state = ST_OUTER;
            break;
            
        case ST_OUTER: //outer picture
            wp = false;
            //find closest place to take picture
            memcpy(target, POIproj, 3*sizeof(float)); //copy the projected POI location to target
            for(int i = 0; i < 3; i++){
                target[i] *= 0.465/mathVecMagnitude(POI,3); //dilate target to outer zone
            }
            mathVecSubtract(facing,origin,target,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            if(pathToTarget(myState,target,waypoint)){
                setWaypoint(waypoint,originalVecBetween);
                state = ST_OUTER + 1;
                for (int i = 0; i < 3; i++) tempTarget[i] = target[i];
            }
            else{
                state = ST_OUTER + 2;
            }
            break;
        
        case ST_OUTER + 1: //something is in the way so go to waypoint
            wp = true;
            mathVecSubtract(facing,POIproj,target,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            if(goToWaypoint(target,waypoint,tempTarget,originalVecBetween)){
                goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
            }
            else{
                state = ST_OUTER + 2;
            }
            break;
            
        case ST_OUTER + 2://go to target
            mathVecSubtract(facing,origin,myState,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            toTarget();
            if(takePic == 0){
                game.takePic(POIID);
            }
            if(game.getMemoryFilled() > checkNum){
                checkNum++;
                getPOILoc(POIproj, POIID, 5);
                state = ST_INNER;
            }
            break;

      
        case ST_INNER: //inner picture
            wp = false;
            memcpy(target, POIproj, 3*sizeof(float));
            for(int i = 0; i < 3; i++){
                target[i] *= 0.34/mathVecMagnitude(POI,3);
            }
            mathVecSubtract(facing,POIproj,target,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            haulAssTowardTarget(target,8);
            state = ST_INNER + 1;
            break;
        
        case ST_INNER + 1: //after outer picture is taken, rush to inner zone. 
            mathVecSubtract(facing,POIproj,myState,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            if(distance(myState,target)<0.02){
                haulAssTowardTarget(target,10);
            }
            else{
                haulAssTowardTarget(target,2);
            }
            if(game.alignLine(POIID)){
                game.takePic(POIID);
            }
            if(game.getMemoryFilled() > checkNum){
                checkNum++;
                state = ST_UPLOAD;
            }
            break;
            
            
        case ST_SHADOW: //shadow zone
            wp = false;
            target[0] = 0.39; //arbitrary point in the shadow zone 
            target[1] = 0.00;
            target[2] = 0.00;
            if(pathToTarget(myState,target,waypoint)){
                setWaypoint(waypoint,originalVecBetween);
                goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
                state = 31;
            }
            else{
                state = 32;
            }
            break;
            
        case ST_SHADOW + 1:
            wp = true;
            if(goToWaypoint(target,waypoint,tempTarget,originalVecBetween)){
                goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
            }
            else{
                toTarget();
                state = 32;
            }
            break;
            
        case ST_SHADOW + 2:
            mathVecSubtract(facing,earth,myState,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            toTarget();
            game.uploadPic();
            break;
           
        case ST_UPLOAD: //upload, needs to be fixed
            for(int i = 0; i < 3; i++){
                uploadPos[i] = myState[i] / mathVecMagnitude(myState, 3) * 0.65;
            }
            mathVecSubtract(facing,earth,uploadPos,3); 
            mathVecNormalize(facing,3);
            api.setAttitudeTarget(facing);
            haulAssTowardTarget(uploadPos,1.8);
            state = ST_UPLOAD + 1;
            break;
            
        case ST_UPLOAD + 1: 
            //get to the closest place, keep trying to upload
            api.setAttitudeTarget(facing);
            if(distance(myState,origin)>0.54){
                api.setPositionTarget(myState);
                game.uploadPic();
            }
            else{
                if(distance(myState,origin)>0.51){
                    api.setPositionTarget(uploadPos);
                }
                else{
                    haulAssTowardTarget(uploadPos,1.8);
                }
            }
            if(picNum == 0){
                DEBUG(("LOOKING FOR POI"));
                state = ST_GET_POI;
            }
            break;
    }
}