Search Algorithm
Predefined position
For all predefined points: Go to predefined point. Scan for ball. If ball is found: Exit search sequence, run catch sequence. Else: Break.
The code doesn't just try one predefined position, but all of them if one does not work. This is efficient because it allows for ultrasound value errors, as well as possible ball movement.
Grid
If ball seen: Exit to catch. Else: Move forward until wall is found. Turn right/left. Move forward 35cm. If wall is hit: Turn ??? Else: Turn right/left. Repeat, turning the opposite direction.
This function gives a 100% chance of finding the ball eventually. It is not particularly fast, but it is thorough.
Catch Algorithm
Scan
Turn -30 degrees. Until total angle is 60 degrees: Turn one degree at a time, for each degree. Save (distance to nearest object, angle) in a list. For minimum distance, return to corresponding angle.
Catch
Until ball is seen: Wait. //Now the ball has been seen. If ball at less that 22 cm: //Check that it's not a wall Move forward 15 cm. If touch sensor at 1 or color sensor sees wall colors: Turn. Restart catch. Else: Close hatch. Move forward slowly: Perform check. If check is ok, go to home. Else open hatch.
Check
For i from 1 to 5: Get distance to ball from ultra sound. If less than 21, increment counter ok by 1. If ok is greater than 3, we have the ball. Else open hatch and restart catch.
The efficiency of the catch sequence is based on the checks: we check first that we are not catching a wall, then once we know it is a ball and the hatch is closed, we check that the ball is indeed within the hatch. The second check may now not work very well, due to te precision of the ultrasound getting worse (the robot may see itsself rather than the ball.
Return Home Algorithm
Begin by defining references before any movement: get the position on the x and y axis (with angular reference), and create dist_x and dist_y, distances travelled on both axes, which will be updated when the robot moves. In forward_plus (the function which is used when the robot needs to go forward): Get the angle, by using compass value and the reference calculated in the beginning. Using the sine and cosine of this angle, calculate the distance travelled on axes x and y, and add it to dist_x and dist_y.In the catch function, when returning home: // First, y axis. Test the sign of dist_y and rotate accordingly. if(dist_y > 0){ turn((-SensorHTCompass(COMPASS_PORT)+ref_y_axis)%360, SensorHTCompass(COMPASS_PORT)); }else{ dist_y=-dist_y; turn((-SensorHTCompass(COMPASS_PORT)+ref_y_axis+180)%360, SensorHTCompass(COMPASS_PORT)); } // Then, just travel the previously computed distance forward_plus(dist_y, 100,SensorHTCompass(COMPASS_PORT)); // Same thing for the other axis. if(dist_x>0){ turn((-SensorHTCompass(COMPASS_PORT)+ref_x_axis+180)%360, SensorHTCompass(COMPASS_PORT)); }else{ dist_x=-dist_x; turn((-SensorHTCompass(COMPASS_PORT)+ref_x_axis)%360, SensorHTCompass(COMPASS_PORT)); } forward_plus(dist_x, 100,SensorHTCompass(COMPASS_PORT)); // We are back in the starting area now...
This algorithm/procedure is efficient because we rely on the data from the COMPASS SENSOR. The angles computed and therefore the "dist_x" and "dist_y" variables are quite accurate.
Listening Algorithm
Listening task, also handles the follower sequence: // Wait for the connection to be established While (BluetoothStatus != NO_ERROR){} // Wait for START MESSAGE While(!getMessage(msg)) // Get the parameters of the game byte rank=msg[5]; byte length=msg[6]; byte previous=msg[7]; byte next_rob=msg[8]; bool follow=true; // If we are the leader, start the leader sequence: find a ball, no need to read other messages if(rank==0){ StartTask(goto_ball_big); follow=false; } // Loop listening for incoming messages. while(follow){ // Wait for a new message while(!getMessage(msg)){} // Message received, what do we need to do? if(msg[3]==ACHILLE){ if(msg[4]==MSG_STOP){ StopAllTasks(); } if(msg[4]==MSG_WAIT){ // Update global variable checked in the function that makes the robot move wait_time=msg[5]; } if(msg[4]==MSG_KICK){ if(msg[5]==0 && msg[7]==ACHILLE){ //Stop follower, start explore, we are the leader StartTask(goto_corner); follow=false; } if(msg[5]==rank+1){ // New follower, get its id next_rob=msg[7]; } if(msg[5]==rank){ // We got kicked StopAllTasks(); } } if(msg[4]==MSG_LEAD){ // Stop follower sequence, exploration begins follow=false; StartTask(goto_corner); } if(msg[4]==MSG_ACTION){ //Get angle, dist, speed turn(angle, ref_y_axis); forward_plus(dist, speed, SensorHTCompass(COMPASS_PORT)); } if(msg[4]==MSG_CANCEL){ cancel=1; } } }