Пример #1
0
void id_bot(struct RoboAI *ai, struct blob *blobs)
{
 // Drive a simple pattern forward/reverse and call the particle filter to
 // find the blobs that are consistent with this in order to identify the
 // bot as well as the opponent.

 double noiseV=5;			// Expected motion magnitude due to noise for a static object
					// (it's just a guess - haven't measured it)
 double oppSizeT=2500;			// Opponent's blob nimimum size threshold
 static double blobData[10][4];	// Keep track of up to 10 blobs that could be
					// the bot. Unlikely there are more since
					// these have to be active tracked blobs.
					// blobData[i][:]=[blobID mx my p]
 static int bdataidx=0;
 double psum;
 double px,siz,len,vx,vy,wx,wy;
 int ownID,i;
 struct blob *oppID;

 struct blob *p;
 static double IDstep=0;		// Step tracker
 double frameInc=1.0/10;		// 1 over the number of frames each step should get

 // Get a handle on the ball
 id_ball(ai,blobs);
 
 // Forward motion for a few frames
 if (IDstep<3*frameInc)
 {
  drive_speed(35);
  clear_motion_flags(ai);
  ai->st.mv_fwd=1;
 }
 else if (IDstep<1.0)
 {
  drive_speed(35);
  clear_motion_flags(ai);
  ai->st.mv_fwd=1;
  particle_filter(ai,blobs);
 }
 else if (IDstep<1.0+(3*frameInc))	// Reverse drive
 {
  drive_speed(-35);
  clear_motion_flags(ai);
  ai->st.mv_back=1;
 }
 else if (IDstep<2.0)
 {
  drive_speed(-35);
  clear_motion_flags(ai);
  ai->st.mv_back=1;
  particle_filter(ai,blobs);  
 }
 else if (IDstep<2.0+(5*frameInc)) {clear_motion_flags(ai); all_stop();}	// Stop and clear blob motion history
 else {IDstep=0; ai->st.state=1;}	// For debug...
 		 
 IDstep+=frameInc; 

}
Пример #2
0
// Provided --------------------------------
int setupAI(int mode, struct RoboAI *ai)
{
	//Note: set state to 0 if it is not used!
	int i;

	switch (mode) {
	case AI_CONFUSED:
		ai->state = 0;
		ai->runAI = confused;
		break;
        case AI_PACO:
		ai->state = NULL;
		ai->runAI = AI_main;
                ai->st.side=0;
		ai->st.state=0;
		all_stop();
		ai->st.ball=NULL;
		ai->st.old_bcx=0;
		ai->st.old_bcy=0;
		ai->st.selfID=0;
		ai->st.oppID=0;
		ai->st.ballID=0;
		clear_motion_flags(ai);
                break;
	default:
		fprintf(stderr, "AI mode %d is not implemented, sorry\n", mode);
		return 0;
	}

	return 1;
}
Пример #3
0
void AI_main(struct RoboAI *ai, struct blob *blobs, void *state)
{
 // Top-level AI

 fprintf (stderr,"AI_main() called for ai at %p and blob list at %p\n",ai,blobs);

 if (ai->st.state==0)  // Initial set up - find own, ball, and opponent blobs			
 {
  fprintf(stderr,"State is 0, self-id in progress...\n");
  id_bot(ai,blobs);
  if (ai->st.selfID==1)
  {
   ai->st.state=1;
   all_stop();
   clear_motion_flags(ai);
  }
 }
}
Пример #4
0
void AI_main(struct RoboAI *ai, struct blob *blobs, void *state)
{
    /*************************************************************************
     This is the main AI loop.

     It is called by the imageCapture code *once* per frame. And it *must not*
     enter a loop or wait for visual events, since no visual refresh will happen
     until this call returns!

     Therefore. Everything you do in here must be based on the states in your
     AI and the actions the robot will perform must be started or stopped
     depending on *state transitions*.

     E.g. If your robot is currently standing still, with state = 03, and
      your AI determines it should start moving forward and transition to
      state 4. Then what you must do is
      - send a command to start forward motion at the desired speed
      - update the robot's state
      - return

     I can not emphasize this enough. Unless this call returns, no image
     processing will occur, no new information will be processed, and your
     bot will be stuck on its last action/state.

     You will be working with a state-based AI. You are free to determine
     how many states there will be, what each state will represent, and
     what actions the robot will perform based on the state as well as the
     state transitions.

     You must *FULLY* document your state representation in the report

     The first two states for each more are already defined:
     State 0,100,200 - Before robot ID has taken place (this state is the initial
               	    state, or is the result of pressing 'r' to reset the AI)
     State 1,101,201 - State after robot ID has taken place. At this point the AI
               	    knows where the robot is, as well as where the opponent and
               	    ball are (if visible on the playfield)

     Relevant UI keyboard commands:
     'r' - reset the AI. Will set AI state to zero and re-initialize the AI
    data structure.
     't' - Toggle the AI routine (i.e. start/stop calls to AI_main() ).
     'o' - Robot immediate all-stop! - do not allow your NXT to get damaged!

     ** Do not change the behaviour of the robot ID routine **
    **************************************************************************/
    double x_distance;
    double y_distance;

    if (ai->st.state==0||ai->st.state==100||ai->st.state==200)  	// Initial set up - find own, ball, and opponent blobs
    {
        // Carry out self id process.
        fprintf(stderr,"Initial state, self-id in progress...\n");
        id_bot(ai,blobs);
        if ((ai->st.state%100)!=0)	// The id_bot() routine will change the AI state to initial state + 1
        {   // if robot identification is successful.
            if (ai->st.self->cx>=512) ai->st.side=1;
            else ai->st.side=0;
            all_stop();
            clear_motion_flags(ai);
            fprintf(stderr,"Self-ID complete. Current position: (%f,%f), current heading: [%f, %f], AI state=%d\n",ai->st.self->cx,ai->st.self->cy,ai->st.self->mx,ai->st.self->my,ai->st.state);
        }
    }
    else
    {
        /****************************************************************************
         TO DO:
         You will need to replace this 'catch-all' code with actual program logic to
         implement your bot's state-based AI.

         After id_bot() has successfully completed its work, the state should be
         1 - if the bot is in SOCCER mode
         101 - if the bot is in PENALTY mode
         201 - if the bot is in CHASE mode

         Your AI code needs to handle these states and their associated state
         transitions which will determine the robot's behaviour for each mode.

         Please note that in this function you should add appropriate functions below
         to handle each state's processing, and the code here should mostly deal with
         state transitions and with calling the appropriate function based on what
         the bot is supposed to be doing.
        *****************************************************************************/
        //fprintf(stderr,"Just trackin'!\n");	// bot, opponent, and ball.
        //track_agents(ai,blobs);		// Currently, does nothing but endlessly track

        if(ai->st.state ==101) {
            x_distance = abs(ai->st.ball->cx - ai->st.self->cx);
            y_distance = abs(ai->st.ball->cy - ai->st.self->cy);
            while(x_distance > 10) {

                drive_speed(50);
                x_distance -= 45;
                all_stop();
                fprintf(stderr,"distance is: %f \n", x_distance);
            }
            ai->st.state += 1;
            fprintf(stderr,"state is %i \n", ai->st.state);
        }
    }

}
Пример #5
0
int setupAI(int mode, int own_col, struct RoboAI *ai)
{
/////////////////////////////////////////////////////////////////////////////
// ** DO NOT CHANGE THIS FUNCTION **
// This sets up the initial AI for the robot. There are three different modes:
//
// SOCCER -> Complete AI, tries to win a soccer game against an opponent
// PENALTY -> Score a goal (no goalie!)
// CHASE -> Kick the ball and chase it around the field
//
// Each mode sets a different initial state (0, 100, 200). Hence,
// AI states for SOCCER will be 0 through 99
// AI states for PENALTY will be 100 through 199
// AI states for CHASE will be 200 through 299
//
// You will of course have to add code to the AI_main() routine to handle
// each mode's states and do the right thing.
//
// Your bot should not become confused about what mode it started in!
//////////////////////////////////////////////////////////////////////////////

    switch (mode) {
    case AI_SOCCER:
        fprintf(stderr,"Standard Robo-Soccer mode requested\n");
        ai->st.state=0;		// <-- Set AI initial state to 0
        break;
    case AI_PENALTY:
        fprintf(stderr,"Penalty mode! let's kick it!\n");
        ai->st.state=100;	// <-- Set AI initial state to 100
        break;
    case AI_CHASE:
        fprintf(stderr,"Chasing the ball...\n");
        ai->st.state=200;	// <-- Set AI initial state to 200
        break;
    default:
        fprintf(stderr, "AI mode %d is not implemented, setting mode to SOCCER\n", mode);
        ai->st.state=0;
    }

    all_stop();			// Stop bot,
    ai->runAI = AI_main;		// and initialize all remaining AI data
    ai->calibrate = AI_calibrate;
    ai->st.ball=NULL;
    ai->st.self=NULL;
    ai->st.opp=NULL;
    ai->st.side=0;
    ai->st.botCol=own_col;
    ai->st.old_bcx=0;
    ai->st.old_bcy=0;
    ai->st.old_scx=0;
    ai->st.old_scy=0;
    ai->st.old_ocx=0;
    ai->st.old_ocy=0;
    ai->st.bvx=0;
    ai->st.bvy=0;
    ai->st.svx=0;
    ai->st.svy=0;
    ai->st.ovx=0;
    ai->st.ovy=0;
    ai->st.selfID=0;
    ai->st.oppID=0;
    ai->st.ballID=0;
    clear_motion_flags(ai);
    fprintf(stderr,"Initialized!\n");

    return(1);
}
Пример #6
0
void AI_main(struct RoboAI *ai, struct blob *blobs, void *state)
{
    /*************************************************************************
     You will be working with a state-based AI. You are free to determine
     how many states there will be, what each state will represent, and
     what actions the robot will perform based on the state as well as the
     state transitions.

     You must *FULLY* document your state representation in the report

     Here two states are defined:
     State 0,100,200 - Before robot ID has taken place (this state is the initial
                       state, or is the result of pressing 'r' to reset the AI)
     State 1,101,201 - State after robot ID has taken place. At this point the AI
                       knows where the robot is, as well as where the opponent and
                       ball are (if visible on the playfield)

     Relevant UI keyboard commands:
     'r' - reset the AI. Will set AI state to zero and re-initialize the AI
    data structure.
     't' - Toggle the AI routine (i.e. start/stop calls to AI_main() ).
     'o' - Robot immediate all-stop! - do not allow your NXT to get damaged!

     ** Do not change the behaviour of the robot ID routine **
    **************************************************************************/

    if (ai->st.state == 0 || ai->st.state == 100 || ai->st.state == 200) // Initial set up - find own, ball, and opponent blobs
    {
        // Carry out self id process.
        fprintf(stderr, "Initial state, self-id in progress...\n");
        id_bot(ai, blobs);
        if ((ai->st.state % 100) != 0) // The id_bot() routine will change the AI state to initial state + 1
        {
            // if robot identification is successful.
            if (ai->st.self->cx[0] >= 512) ai->st.side = 1; else ai->st.side = 0;
            all_stop();
            clear_motion_flags(ai);
            fprintf(stderr, "Self-ID complete. Current position: (%f,%f), current heading: [%f, %f], AI state=%d\n", ai->st.self->cx[0], ai->st.self->cy[0], ai->st.self->mx, ai->st.self->my, ai->st.state);
        }
    }
    else
    {
        /****************************************************************************
         TO DO:
         You will need to replace this 'catch-all' code with actual program logic to
         have the robot do its work depending on its current state.
         After id_bot() has successfully completed its work, the state should be
         1 - if the bot is in SOCCER mode
         101 - if the bot is in PENALTY mode
         201 - if the bot is in CHASE mode

         Your AI code needs to handle these states and their associated state
         transitions which will determine the robot's behaviour for each mode.
        *****************************************************************************/
        // track_agents(ai,blobs);        // Currently, does nothing but endlessly track
        // fprintf(stderr,"Just trackin'!\n");    // bot, opponent, and ball.

        switch(ai->st.state){
            case 101:

                kick();
                sleep(2);
                break;
            case 201:
                chase(ai);
                break;
        }
    }

}
Пример #7
0
void AI_main(struct RoboAI *ai, struct blob *blobs, void *state)
{
  /*************************************************************************
  This is the main AI loop.

  It is called by the imageCapture code *once* per frame. And it *must not*
  enter a loop or wait for visual events, since no visual refresh will happen
  until this call returns!

  Therefore. Everything you do in here must be based on the states in your
  AI and the actions the robot will perform must be started or stopped 
  depending on *state transitions*. 
  E.g. If your robot is currently standing still, with state = 03, and
   your AI determines it should start moving forward and transition to
   state 4. Then what you must do is 
   - send a command to start forward motion at the desired speed
   - update the robot's state
   - return

  I can not emphasize this enough. Unless this call returns, no image
  processing will occur, no new information will be processed, and your
  bot will be stuck on its last action/state.
  You will be working with a state-based AI. You are free to determine
  how many states there will be, what each state will represent, and
  what actions the robot will perform based on the state as well as the
  state transitions.
  You must *FULLY* document your state representation in the report
  The first two states for each more are already defined:
  State 0,100,200 - Before robot ID has taken place (this state is the initial
                  state, or is the result of pressing 'r' to reset the AI)
  State 1,101,201 - State after robot ID has taken place. At this point the AI
                  knows where the robot is, as well as where the opponent and
                  ball are (if visible on the playfield)
  Relevant UI keyboard commands:
  'r' - reset the AI. Will set AI state to zero and re-initialize the AI
  data structure.
  't' - Toggle the AI routine (i.e. start/stop calls to AI_main() ).
  'o' - Robot immediate all-stop! - do not allow your NXT to get damaged!
  ** Do not change the behaviour of the robot ID routine **
  **************************************************************************/

  if (ai->st.state==0||ai->st.state==100||ai->st.state==200)   // Initial set up - find own, ball, and opponent blobs
  {
    // Carry out self id process.
    fprintf(stderr,"Initial state, self-id in progress...\n");
    id_bot(ai,blobs);
    if ((ai->st.state%100)!=0)  // The id_bot() routine will change the AI state to initial state + 1
    {       // if robot identification is successful.
      if (ai->st.self->cx>=512) ai->st.side=1; else ai->st.side=0;
      all_stop();
      clear_motion_flags(ai);
      fprintf(stderr,"Self-ID complete. Current position: (%f,%f), current heading: [%f, %f], AI state=%d\n",ai->st.self->cx,ai->st.self->cy,ai->st.self->mx,ai->st.self->my,ai->st.state);
    }
  } else {
    track_agents(ai,blobs);  

    // reserve first three states in each mode for forward drive direction determination
    if ((ai->st.state > 0 && ai->st.state < 3) || 
        (ai->st.state > 100 && ai->st.state < 103) ||
        (ai->st.state > 200 && ai->st.state < 203)) {
      clear_motion_flags(ai);
      drive_speed(50);
      ai->st.state++;
    } 
    // otherwise, go to respective state mode and navigate
    else {
      // save up to date blob info to avoid segfaults
      backupReadings(ai);

      // SOCCER MODE
      if (ai->st.state >= 3 && ai->st.state <= 100) {
        switch(ai->st.state) {
          case 3: 
            if (checkOpponent(ai, 1)){
              approachTargetLocation(ai, TO_KICK_POS);
            } else {
              approachTargetLocation(ai, TO_DEFENSE);
            }
            break;
          case 4: 
            // if ball has moved by opponent or earthquake, jump back a state
            if ((ballHistX[0] - ballHistX[1] > 10.0) || (ballHistY[0] - ballHistY[1] > 10.0))
              --ai->st.state;
            approachTargetLocation(ai, TO_BALL); 
            break;
          case 5: kickBall(ai); break;
          default: ai->st.state = 1; break;
        }
      }
      // PENALTY MODE
      else if (ai->st.state >= 103 && ai->st.state <= 200) {
        switch(ai->st.state) {
          case 103: approachTargetLocation(ai, TO_KICK_POS); break;
          case 104: approachTargetLocation(ai, TO_BALL); break;
          case 105: kickBall(ai); break;
          default: ai->st.state = 101; break;
        }
      }
      // CHASE MODE
      else if (ai->st.state >= 203 && ai->st.state <= 300) {
        switch(ai->st.state) {
          case 203: approachTargetLocation(ai, TO_BALL); break;
          case 204: kickBall(ai); break;
          default: ai->st.state = 201; break;
        }
      }
    }
  }
}