void ContainerGrabber::initGrabSequence() {
setControlMode(leftArm, "position");
setControlMode(rightArm, "position");
setPIDslot(leftArm, 0);
setPIDslot(rightArm, 0);
setPositionTarget(leftArm, dropTargetAngle);
setPositionTarget(rightArm, dropTargetAngle);
}
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);
}
}
}