void id_bot(struct RoboAI *ai, struct blob *blobs)
{
///////////////////////////////////////////////////////////////////////////////
// ** DO NOT CHANGE THIS FUNCTION **
// This routine calls track_agents() to identify the blobs corresponding to the
// robots and the ball. It commands the bot to move forward slowly so heading
// can be established from blob-tracking.
//
// NOTE 1: All heading estimates, velocity vectors, position, and orientation
// are noisy. Remember what you have learned about noise management.
//
// NOTE 2: Heading and velocity estimates are not valid while the robot is
// rotating in place (and the final heading vector is not valid either).
// To re-establish heading, forward/backward motion is needed.
//
// NOTE 3: However, you do have a reliable orientation vector within the blob
// data structures derived from blob shape. It points along the long
// side of the rectangular 'uniform' of your bot. It is valid at all
// times (even when rotating), but may be pointing backward and the
// pointing direction can change over time.
//
// You should *NOT* call this function during the game. This is only for the
// initialization step. Calling this function during the game will result in
// unpredictable behaviour since it will update the AI state.
///////////////////////////////////////////////////////////////////////////////
struct blob *p;
static double stepID=0;
double frame_inc=1.0/5.0;
drive_speed(30); // Start forward motion to establish heading
// Will move for a few frames.
track_agents(ai,blobs); // Call the tracking function to find each agent
if (ai->st.selfID==1&&ai->st.self!=NULL)
fprintf(stderr,"Successfully identified self blob at (%f,%f)\n",ai->st.self->cx,ai->st.self->cy);
if (ai->st.oppID==1&&ai->st.opp!=NULL)
fprintf(stderr,"Successfully identified opponent blob at (%f,%f)\n",ai->st.opp->cx,ai->st.opp->cy);
if (ai->st.ballID==1&&ai->st.ball!=NULL)
fprintf(stderr,"Successfully identified ball blob at (%f,%f)\n",ai->st.ball->cx,ai->st.ball->cy);
stepID+=frame_inc;
if (stepID>=1&&ai->st.selfID==1) // Stop after a suitable number of frames.
{
ai->st.state+=1;
stepID=0;
all_stop();
}
else if (stepID>=1) stepID=0;
// At each point, each agent currently in the field should have been identified.
return;
}
void id_bot(struct RoboAI *ai, struct blob *blobs)
{
// ** DO NOT CHANGE THIS FUNCTION **
// This routine calls track_agents() to identify the blobs corresponding to the
// robots and the ball. It commands the bot to move forward slowly so heading
// can be established from blob-tracking.
//
// NOTE 1: All heading estimates are noisy.
//
// NOTE 2: Heading estimates are only valid when the robot moves with a
// forward or backward direction. Turning destroys heading data
// (why?)
//
// You should *NOT* call this function during the game. This is only for the
// initialization step. Calling this function during the game will result in
// unpredictable behaviour since it will update the AI state.
struct blob *p;
static double stepID = 0;
double frame_inc = 1.0 / 5.0;
drive_speed(30); // Need a few frames to establish heading
track_agents(ai, blobs);
if (ai->st.selfID == 1 && ai->st.self != NULL)
fprintf(stderr, "Successfully identified self blob at (%f,%f)\n", ai->st.self->cx[0], ai->st.self->cy[0]);
if (ai->st.oppID == 1 && ai->st.opp != NULL)
fprintf(stderr, "Successfully identified opponent blob at (%f,%f)\n", ai->st.opp->cx[0], ai->st.opp->cy[0]);
if (ai->st.ballID == 1 && ai->st.ball != NULL)
fprintf(stderr, "Successfully identified ball blob at (%f,%f)\n", ai->st.ball->cx[0], ai->st.ball->cy[0]);
stepID += frame_inc;
if (stepID >= 1 && ai->st.selfID == 1)
{
ai->st.state += 1;
stepID = 0;
all_stop();
}
else if (stepID >= 1) stepID = 0;
return;
}
void AI_calibrate(struct RoboAI *ai, struct blob *blobs)
{
// Basic colour blob tracking loop for calibration of vertical offset
// See the handout for the sequence of steps needed to achieve calibration.
track_agents(ai,blobs);
}
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;
}
}
}
}
}
