YUYU THE BASKETBALL PLAYER
The Algorithm
Source Code
You can download my source code from here.
> Initialization
//The distance to define whether an object is detected or not
#define NEAR 30
// 0 for blue basket in front or 1 for blue basket in Back
#define DIRECTION 1
bool object_detected; // boolean for ultra sonic; true when distance is smaller than NEAR
bool ball_found; // boolean for color; true when red or blue detected
bool hit_wall; // boolean for whether hit a wall;
long time_allstart; // The start time for each round
int angle_init; // The compass value of the start point
int angle_direction; // The compass value of the basket
long time_init; //The start time for choice 2
long time_duration; // The go straight duration for choice 2
bool make_choice_now; // The boolean for making choice or not; true for making a choice
bool timerstarted; // The timerstarted boolean for choice 2
bool justbegin; // The boolean for whether the beginning of the round or not
// set sensors ports
void set_sensors(){
SetSensorColorFull(IN_4);
SetSensorLowspeed(IN_1); // eye
SetSensorTouch(IN_3);
SetSensorLowspeed(IN_2); // compass
}
> Check the color
set to true when the value of the ultra light sensor is less than 30 cm
/*The task for checking color sensor, if red or blue is detected, set the
boolean ball_found to true and value of the angle_direction */
task check_color(){
while(true){
if(Sensor(IN_4)==INPUT_REDCOLOR){
ball_found = true;
angle_direction= (angle_init+(1-DIRECTION)*180)%360;
TextOut(10,30,"Red ball");
}else if(Sensor(IN_4)==INPUT_BLUECOLOR){
ball_found = true;
angle_direction= (angle_init+DIRECTION*180)%360;
TextOut(10,30,"Blue ball");
}else{
ball_found = false;
}
}
}
> Check if Hit Wall
set to true, when the value of touch sensor is 1, or the value of ultra light sensor stays less than 10 cm for 5 seconds (in case the robot is blocked without the value of the touch sensor is 1 ).
/*The task for checking hit wall, hit_wall is set to true when either the value of
touch sensor is 1 or the value of ultra sonic sensor stays smaller than 10cm for 5 seconds*/
task check_hit_wall(){
while(true){
if(Sensor(IN_3) == 1){
hit_wall = true;
ClearScreen();
TextOut(10,30,"Hit_wall");
}else if(SensorUS(IN_1)< 10 ){
long time_t0 = CurrentTick();
bool timeout = false;
while(Sensor(IN_3)!=1 && SensorUS(IN_1)<10 && (timeout = !((CurrentTick()-time_t0)<5000)));
if(Sensor(IN_3)==1 || timeout){
timeout = false;
hit_wall = true;
ClearScreen();
TextOut(10,30,"Hit_wall");
}
}else{
hit_wall = false;
}
}
}
> Check if Object Detected
set to true when the value of the ultra light sensor is less than 30 cm
/* The task for checking the distance, set object_detected true when the value of
ultra sonic sensor is less than NEAR*/
task check_object(){
while(true){
if(SensorUS(IN_1)<=NEAR){
object_detected = true;
}else{
object_detected = false;
}
}
}
> Go Straight
/*Since our robot has a problem to go straight, so this function is to adapte motors'
speed to make the robot go straight as possible*/
void go_straight(){
OnFwd(OUT_A,98);
OnFwd(OUT_B,100);
}
> Move To The Basket
this function will be called every 5 seconds if no ball is found. In this move, the object detected value is not checked. If a ball is not found, the robot will go straight. If the robot hits a wall, it will turn left a little bit (to make sure that it goes against the wall). After 8 times hitting wall, this function finished.
/*This function is to find the way to the right basket and throw the ball.After that the
robot turns 180 and start for a new round*/
void move_back(){
ClearScreen();
TextOut(30,20,"move_back");
NumOut(30,30,angle_direction);
int angle = SensorHTCompass(IN_2)%360;
int angle_diff =angle-angle_direction;
NumOut(30,30,angle);
NumOut(30,50,angle_diff);
// find the direction
while(abs(angle_diff)>=5){
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(20);
NumOut(30,30,SensorHTCompass(IN_2)%360);
NumOut(30,50, angle_diff);
angle_diff= SensorHTCompass(IN_2)%360 - angle_direction;
}
Off(OUT_AB);
Wait(500);
// Forward until hit the wall
go_straight();
while(!hit_wall);
Off(OUT_AB);
// go back a little bit
OnRev(OUT_AB,50);
Wait(700);
//turn left
Off(OUT_AB);
ClearScreen();
TextOut(20,30,"Turn to adjust the distance.");
int angle_orthogonal = angle_direction+95;
int angle_current = SensorHTCompass(IN_2)%360;
while(abs(angle_current-angle_orthogonal)>5){
OnFwd(OUT_A,50);
OnRev(OUT_B,50);
Wait(20);
angle_current = SensorHTCompass(IN_2)%360;
}
Off(OUT_AB);
// Adjust the distance to make the value of the ultra sonic be 44cm
int dist_wall = SensorUS(IN_1);
ClearScreen();
TextOut(20,30,"Adjust the distance.");
repeat(2){
if(dist_wall<44){
OnRev(OUT_A,49);
OnRev(OUT_B,50);
while(SensorUS(IN_1)<44);
Off(OUT_AB);
}else{
OnFwd(OUT_A,49);
OnFwd(OUT_B,50);
while(SensorUS(IN_1)>44);
Off(OUT_AB);
}
}
//After finding the right distance, turn to the basket,
//forward until the wall
angle_current = SensorHTCompass(IN_2)%360;
long t0_turn = CurrentTick();
//while(abs(angle_current-angle_direction-10)>5){
while(CurrentTick()-t0_turn<=2000){
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(20);
angle_current = SensorHTCompass(IN_2)%360;
}
Off(OUT_AB);
go_straight();
while(!hit_wall);
Off(OUT_AB);
Wait(200);
// throw the ball
RotateMotor(OUT_C,50,40);
Wait(20);
RotateMotor(OUT_C,50,100);
RotateMotor(OUT_C,75,-140);
Wait(500);
//Turn 180 degrees
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(4500);
time_allstart = CurrentTick(); // restart from the beginning
justbegin = true;
}
> Specific Actions
/*The moves of the arm after a ball is found to make sure the ball is well protected */
void ball_found_action(){
Off(OUT_AB);
OnFwd(OUT_C,50);
Wait(200);
Off(OUT_C);
Wait(500);
}
/*The moves after an object detected. Turn left a little bit and forword */
void object_detected_action(){
Off(OUT_AB);
int dist = Sensor(IN_1);
OnFwd(OUT_B,50);
OnRev(OUT_A,50);
Wait(200);
OnFwdSync(OUT_AB,80,0);
OnFwd(OUT_C,50);
long t0 = CurrentTick();
while(!ball_found && (CurrentTick()-t0)<=3000 && !hit_wall);
}
> Go Along the Wall
this function will be called every 5 seconds if no ball is found. In this move, the object detected value is not checked. If a ball is not found, the robot will go straight. If the robot hits a wall, it will turn left a little bit (to make sure that it goes against the wall). After 8 times hitting wall, this function finished.
/*The move to go along the wall, quit the function after hitting walls 8 times*/
void go_along_wall(){
ClearScreen();
TextOut(30,30,"go_along the wall !!!!");
int hitwalltime = 0 ;
while(hitwalltime <= 8){
go_straight();
OnFwd(OUT_C,50);
if(ball_found){
ball_found_action();
move_back();
}
if(hit_wall){
hitwalltime ++;
ClearScreen();
TextOut(20,20,"!!!!!!!Turn");
RotateMotor(OUT_C,50,-180);
Off(OUT_AB);
int angle_before_turn = SensorHTCompass(IN_2)%360;
Wait(200);
int angle_after_turn = (angle_before_turn-55)%360;
long time_1 = CurrentTick();
while(abs(SensorHTCompass(IN_2)%360 - (angle_before_turn-55)%360)>=2 && (CurrentTick()-time_1)<=3000){
OnFwd(OUT_B,50);
OnRev(OUT_A,50);
}
}
}
}
> Find Ball in the Middle Line
This function will be called just at the beginning or each time the start of searching. The robot will go straight to the middle, turn to one side until hitting a wall. Then it will turn 180 degree and go straight until hitting the wall. During this move, anytime if a ball is found, it will stop and start moving to the basket.
/*The function to grab balls on the middle line*/
void findBallinMiddle() {
TextOut(1,30,"Move and search.");
go_straight();
OnFwd(OUT_C,50);
long t0=CurrentTick();
while( (CurrentTick()-t0)<4200 && !(hit_wall) && ball_found==false);
Off(OUT_AB);
if(ball_found) {
Off(OUT_C);
// move_back();
} else {
// turn to one direction
OnFwd(OUT_A,80);
OnRev(OUT_B,80);
int one_direction = (angle_init + 95 ) % 360;
while(abs(SensorHTCompass(IN_2) - one_direction ) >= 2 && ball_found==false);
Off(OUT_AB);
if (ball_found) {
Off(OUT_C);
//move_back();
}
else {
OnFwdSync(OUT_AB,80,0);
while(!(hit_wall) && ball_found==false);
Off(OUT_AB);
if(ball_found) {
Off(OUT_C);
// move_back();
}
else {
// turn to another direction
int other_direction = (angle_init-92 + 360) % 360;
OnRev(OUT_A,80);
OnFwd(OUT_B,80);
while(abs(SensorHTCompass(IN_2) - other_direction ) >= 2 && ball_found==false);
Off(OUT_AB);
if(ball_found) {
Off(OUT_C);
// move_back();
}
else {
OnFwdSync(OUT_AB,80,0);
while(!(hit_wall) && ball_found==false);
Off(OUT_AB);
if(ball_found) {
Off(OUT_C);
// move_back();
}
}
}
}
}
}
> Moving Main Task
this task contents moves of the robot. It checks global values to call corresponding functions. Inside, if no ball found, no hit wall, no object detected, no go along the wall. It will make a random choice between going straight and going straight during some time and scanning (turn a complete 360, if no ball found , turn a certain degree to go).
/*The task for searching and moves*/
task move(){
time_allstart = CurrentTick();
while(true){
if(justbegin){ //At the beginning of each round, go check the middleline
justbegin = false;
findBallinMiddle();
}
if(make_choice_now){ //Make a random choice
TextOut(30,30,"make choice now!!");
int timerOrNot = Random(2);
//NumOut(50,50,timerOrNot);
if(timerOrNot == 1){
time_duration = Random(5000)+3000;
NumOut(20,30,time_duration);
timerstarted = true;
time_init = CurrentTick();
make_choice_now = false;
}
}
go_straight();
OnFwd(OUT_C,50);
// Check values of global variables and make moves
if(ball_found){
make_choice_now = true;
timerstarted = false;
ball_found_action();
move_back();
}
if(object_detected){
TextOut(30,30,"See something!!");
make_choice_now = true;
timerstarted = false;
object_detected_action();
}
if(hit_wall){
make_choice_now = true;
timerstarted = false;
int turn_time = Random(2500)+1000;
ClearScreen();
TextOut(50,50,"hit a wall");
OnRev(OUT_AB,50);
RotateMotor(OUT_C,50,-180);
Wait(1000);
Off(OUT_AB);
int angle_before_turn = SensorHTCompass(IN_2)%360;
Wait(200);
OnFwd(OUT_B,50);
OnRev(OUT_A,50);
Wait(500); // turn a little bit for deblock the arm
OnFwd(OUT_C,50);
Wait(turn_time-500);
Off(OUT_AB);
int angle_after_turn = SensorHTCompass(IN_2)%360;
Wait(200);
//If the left is blocked.turn right
if(abs(angle_after_turn-angle_before_turn)<20){
OnFwd(OUT_A,50);
OnRev(OUT_B,50);
turn_time = Random(3000)+500;
Wait(turn_time);
Off(OUT_AB);
angle_after_turn = SensorHTCompass(IN_2)%360;
Wait(200);
// if the right is blocked. go back.
if(abs(angle_after_turn-angle_before_turn)<20){
OnRev(OUT_AB,70);
Wait(3000);
OnFwd(OUT_B,50);
OnRev(OUT_A,50);
}
}
}
if((CurrentTick() - time_allstart)>=5000){ // After 5 seconds, go along the wall
NumOut(30,30,time_allstart);
NumOut(50,30,CurrentTick());
make_choice_now = true;
timerstarted = false;
TextOut(30,30,"go_along the wall !!!!");
time_allstart = CurrentTick();
go_along_wall();
}
if(timerstarted && CurrentTick() - time_init > time_duration){
TextOut(30,30,"scan !!!!");
make_choice_now = true;
timerstarted = false;
bool stopturn = false;
//scan and turn
int angle_current = SensorHTCompass(IN_2)%360;
int angle_turn = (SensorHTCompass(IN_2)+180)%360;
while(abs(angle_current-angle_turn)>5){
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(20);
angle_current = SensorHTCompass(IN_2)%360;
if(hit_wall || ball_found || object_detected){
stopturn = true;
Off(OUT_ABC);
if(object_detected){
TextOut(30,30,"Turn more !!!!");
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(500);
}
break;
}
}
if(!stopturn){
angle_turn = (angle_turn+180)%360;
angle_current = SensorHTCompass(IN_2)%360;
while(abs(angle_current-angle_turn)>5){
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(20);
angle_current = SensorHTCompass(IN_2)%360;
if(hit_wall || ball_found || object_detected){
stopturn = true;
Off(OUT_ABC);
if(object_detected){
TextOut(30,30,"Turn more !!!!");
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(500);
}
break;
}
}
}
if(!stopturn){
OnRev(OUT_A,50);
OnFwd(OUT_B,50);
Wait(1000);
}
}
}
}
> Main Task
There are four tasks. Three of them are used for checking sensor states and updating the values of global variables (ball_found, object_detected, hit_wall). The other task is for rotating motors. So we do not have synchronizing problem, we do not have to use mutex. We use different functions to realize different moves.
/*Main task*/
task main(){
// Initialize variables
justbegin = true;
object_detected = false;
ball_found = false;
hit_wall = false;
make_choice_now = true;
timerstarted = false;
set_sensors();
angle_init = SensorHTCompass(IN_2);
Precedes(check_color,check_hit_wall,check_object,move);
}
The Instructions
Under Linux
- Download the NXC package on this site.
- Edit your NXC source code in your favourite Text Editor
- Save it in some folder
- To compile your file and check for errors, in a shell enter the command :
nbc your_source_code.nxc - To compile and download your file on the robot, plug the controller cable and enter:
nbc -d your_source_code.nxc