task main()
{
int t;
int direction= TURN_RIGHT;
while(true)
{
//estado = camino
goFast();
while(SensorValue[lightSensor] < LIGHT_WHITE);
goStop();
//estado = correccion optimista
//giro 1
t= TURN_INIT_TIME;
turn(direction);
if(stopWhenBlackOrTimeout(t))
continue;
//giro 2
turn*= -1;
turn(direction);
if(stopWhenBlackOrTimeout(t*2))
continue;
//estado = correccion pesimista
//deshace giro
direction*= -1;
turn(direction);
wait1Msec(t);
goStop();
//regresa
goBackwards();
while(SensorValue[lightSensor] >= LIGHT_WHITE);
goStop();
//find other wall
while(SensorValue[lightSensor] < LIGHT_WHITE)
{
turn(direction);
wait1Msec(t);
goStop();
t+=20;
direction*=-1;
}
//center in path
turn(-direction);
wait1Msec(CENTER_TIME);
goStop();
}
}
unsigned char TestDog(char reset) {
// Returns 2 if it finds the dog while walking 20 cm to the front
static int state = 0;
if (reset) {
state = 0;
} else {
switch (state) {
case 0:
go('F');
goSlow();
MotorsON = 1;
//step_counter[1] = 30 * SPC_front;
//countingDirection = 'F';
state = 99;
break;
case 99:
if (backDistance > 22) {
go('L');
state = 1;
}
case 1:
if (leftDistance < 6) {
state = 2;
}
if (frontDistance < 40) {
state = 6; // DOG FOUND
}
break;
case 2:
go('R');
state = 3;
break;
case 3:
if (leftDistance > 20) {
state = 5;
}
if (frontDistance < 40) {
state = 6; // DOG FOUND
}
break;
/*
case 4:
if (step_counter[0] == 0) {
go('F');
state = 1;
}
break;
*/
case 5:
MotorsON = 0;
state = 0;
goFast();
return 1; // Dog was not detected
break;
case 6:
MotorsON = 0;
state = 0;
goFast();
return 2; // Dog was detected
break;
}
}
return 0;
}
void loop(){
api.getMyZRState(self); //get my own location and save to self
api.getOtherZRState(other); //get opponent's location and save to other
if (counter < 2)
{
itemNum = 0;
otherNum = 1;
if(self[0]<0){
itemNum=1;
otherNum=0;
}
}
stayInBounds();
if(step==acquireLaser){ //fist step
stayInBounds();
if(game.haveItem(1, itemNum)||counter==68){
if(otherNum!=itemNum){
atarget[0]=-atarget[0];
otherNum=1-otherNum;
}
else{
step = GravityPull;
}
}
if(counter > 28 && !game.isNetBroken())
{
if(otherNum!=itemNum){
atarget[0]=-atarget[0];
otherNum=1-otherNum;
}
else if (!game.haveItem(1, otherNum))
{
if(otherNum!=itemNum){
atarget[0]=-atarget[0];
otherNum=1-otherNum;
}
}
else{
step = GravityPull;
}
}
debrisOnPath=CheckPath(atarget); //check if any debris is on way to laser pack
if(debrisOnPath > -1){ //if so
if(distance(self, debrisArr[debrisOnPath])< 0.32f){ //slow down as you approach the debris
mathVecSubtract(down, self, debrisArr[debrisOnPath], 3);
down[1]=0;
mathVecNormalize(down, 3);
down[0]*=3;
down[2]*=3;
api.setVelocityTarget (down);
}
else{
goFast(atarget, 0.07f);
}
}
else{ //if no debris on path, go straight to laser
api.setPositionTarget(atarget);
arbState[0] = self[3];
arbState[1] = self[4];
arbState[2] = self[5];
//DEBUG(("%f",mathVecMagnitude(arbState, 3)));
if(distance(self, atarget)< 0.05f && mathVecMagnitude(arbState, 3) < 0.01f){ //if close enough to laser pack
step=spin;
}
}
}
else if(step == spin){
aimToPickUpLaser[0]=0.0f;
aimToPickUpLaser[1]=0.0f;
aimToPickUpLaser[2]=0.3f;
api.setAttRateTarget(aimToPickUpLaser); //spin robot to pick up laser pack
api.setPositionTarget(atarget); //mantain position on top of laser pack
if(game.haveItem(0, otherNum)){ //go to next step if laser pack is acquired
if(other[1] > 0.3f) {
step=smasherBot;
}
else {
step = moveToShootPos;
}
atarget[0]=-0.05f;//-0.35;//0.1;//0.30;//0.33;
atarget[1]=0.6f;//0.15;//0.3;//0.44;//0.29;
atarget[2]=0.1f;//0.1;//0;//-0.2;//-0.31;//-0.48;
if(itemNum==1){
atarget[2]=-atarget[2];
}
down[0] = 0.0f;
down[1] = 0.0f;
down[2] = 0.0f;
}
}
else if(step==moveToShootPos){ // if second step
game.getCometState(0, arbState);
//DEBUG(("%f, %f, %f", arbState[0], arbState[1], arbState[2]));
mathVecSubtract(aimVector,arbState,self,3); //find vector between self and the starting position of the comet
atarget[0] = arbState[0] - 0.35f;
if(itemNum == 1){
atarget[0] = arbState[0] + 0.35f;
}
debrisOnPath=CheckPath(atarget); //check for any debris on path to Shooting location
if(debrisOnPath > -1){ //if there is, get it
if(distance(self, debrisArr[debrisOnPath])< 0.3f){ //slow down as you approach the debris
mathVecSubtract(down, self, debrisArr[debrisOnPath], 3);
down[0]=0.0f;
down[1]=0.0f;
mathVecNormalize(down, 3);
api.setForces(down);
}
else{
api.setPositionTarget(atarget);
}
}
else{ //if there isn't, go straight to shooting Location
api.setPositionTarget(atarget);
}
api.setAttitudeTarget(aimVector); //constantly be adjusting attitude to face starting position of comet
if(counter>87){
down[0] = 0.015f;
down[1] = -0.026f;//23;
down[2] = 0.005f;
if(itemNum == 1){
down[0] *= -1;
down[2] *= -1;
}
api.setVelocityTarget(down);
}
if(counter==90){ //wait for phase two to begin and go onto next step
step=shootLaser;
}
}
if(step==shootLaser){ //phase two: shooting lasers
game.getCometState(0, arbState); //get position of comet
//game.predictCometState(3, arbState, finState); //predict position of comet in three seconds (gives more sooting accuracy)
mathVecSubtract(aimVector,arbState,self,3); //find vector between self and comet's [predicted] position
if(counter>92){
down[0] = 0.015f;
down[1] = -0.025f;//23;
down[2] = 0.01f;
if(itemNum == 1){
down[0] *= -1;
down[2] *= -1;
}
api.setVelocityTarget(down);
}
api.setAttitudeTarget(aimVector); //constantly adjust attitude to aim at the comet
if(isInShot()){
hit = game.shootLaser(); //shoot laser
if(hit){ //print whether shot was successful
DEBUG (("Hit on B5")); //joke referencing the game Battlefield
}
else{ //print if shot missed
DEBUG (("MISS"));
}
}
//api.setPositionTarget(shootPos);
if(game.laserShotsRemaining()==0){ //once out of laser shots, go onto next step
step=smasherBot;
}
}
if(step==smasherBot){ // once finished shooting comets, play SmasherBot
stayInBounds();
goFast(other, 0.09);
if(counter>64&&counter<90){
stayInBounds();
step = moveToShootPos;
}
}
/*
if(step == antiSmasherBot){
findClosestDebris();
if(game.haveDebris(0,x)){
DEBUG(("Gotcha"));
debCounter++;
}
x=closestDebris;
DEBUG((" x is: %d", closestDebris));
goFast(debrisArr[x],0.06);
game.getCometState(0, arbState);
gravityStartPos[0] = arbState[0] + 0.15;
gravityStartPos[2] = arbState[2] - 0.05;
if(itemNum == 1){
gravityStartPos[0] = arbState[0] - 0.15;
gravityStartPos[2] = arbState[2] + 0.05;
}
if(debCounter==2 || counter>64){
step=GravityPull;
}
}
*/
if(step == GravityPull){
if(counter<91){
gravityStartPos[0] = 0.45;//arbState[0] + 0.15f;
gravityStartPos[2] = arbState[2] - 0.05f;
if(itemNum == 1){
gravityStartPos[0] = -0.45;//arbState[0] - 0.15f;
gravityStartPos[2] = arbState[2] + 0.05f;
}
debrisOnPath=CheckPath(gravityStartPos); //check if any debris is on way to laser pack
if(debrisOnPath > -1){ //if so
if(distance(self, debrisArr[debrisOnPath])< 0.3f){ //slow down as you approach the debris
mathVecSubtract(down, self, debrisArr[debrisOnPath], 3);
down[1]=0;
mathVecNormalize(down, 3);
api.setForces(down);
}
else{
goFast(gravityStartPos, 0.06f);
}
}
else{ //if no debris on path, go straight to laser
goFast(gravityStartPos, 0.06f);
}
}
else{
game.getCometState(0, arbState); //get position of comet
//game.predictCometState(3, arbState, finState);
atarget[0] = arbState[0] + 0.2f;
if(self[0]<0){
atarget[0] = arbState[0] - 0.2f;
}
atarget[1] = arbState[1] - 0.1f;
atarget[2] = arbState[2];
goFast(atarget, 0.09f);
}
}
counter++;
}
