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);
}
}
}
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;
}
}
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;
}
}