/* * Assignment #B, CS154 Spring 2001 * Name : SooYoung Jung * Date : Feb, 22, 2001 * File : bug1.cc * */ /* * bug1.cc * * This is a skeleton file for controlling the Nomad * simulator in Assignment A on various reactive control strategies. * * Feel free to use the makefile in this directory also. * */ #include #include #include #include "KbdInput.hh" extern "C" { #include "Nclient.h" } /****************************************************************** * * This function gets the shorte * fromSonar to toSonar * * The index of that minimum reading is returned. * * Because I think of the sonars as being numbered starting from * zero, there is a conversion to the actual location of the * sonar values in the State vector. * * Note: though input values may be anything bigger than -32, the * index returned is the actual indes into the state vector, * i.e., between 17 and 32 inclusive. * ******************************************************************/ /* * class Point to help to remember the position. */ class Point { public: Point():x(0), y(0){}; Point(int x_, int y_): x(x_), y(y_) {}; ~Point() {}; int getX() const { return x; }; int getY() const { return y; }; void operator=(Point what) { x = what.x; y = what.y; } bool operator==(Point right) { return (x == right.x && y == right.y); } private: int x; int y; }; const int LEFT = 1; const int RIGHT = -1; const int TENTH = 10; // need to make the angle to 10th.. const int RIGHT_ANGLE = 900; const float PI = 3.14159265; const int LESS_CLOSE = 100; // Distance to obstacle const int CLOSE = 50; const int VERY_CLOSE = 25; const int MINIMUM = 5; const int ADD_UP_MAX = 75; // Maximum value of one quater sensers // addUp. double lengthIs(Point, Point); double angleIs(Point, Point); void turnTo(int, int); bool turing(int); int addUp(int, int); int getShortestSonarIndex(int fromSonar, int toSonar) { int realSN; /* real sonar number in the State vector */ int minSonar = 1000; /* higher than any real sonar value */ int minSonarIndex; /* swap from and to, if they're not in order */ if (fromSonar > toSonar) { fromSonar ^= toSonar ^= fromSonar ^= toSonar; } for (int i=fromSonar ; i 1) /* a different velocity can be sent in */ { tv = atoi(argv[1]); cout << "Setting translational velocity to " << tv << endl; } /* * Getting destination point. */ Point start(0, 0); Point end(2750, 4200); if (argc > 3) { end = Point(atoi(argv[2]), atoi(argv[3])); cout << "Setting end point to (" << argv[2] << ", " << argv[3] << ")" << endl; } /* * connect to the simulator */ if (connect_robot(1)) { printf("Connected to robot.\n"); printf("Hit q to exit.\n"); } else { printf("Failed to connect to robot\n"); exit(0); } /* * zero the robot */ zr(); sp(200, 450, 450); /* * turn on appropriate sensors */ for (int i=17 ; i<=32 ; ++i) Smask[i] = 1; ct(); gs(); /* update the robot's state */ int rv = 0; /* start going straight ahead */ vm(tv,rv,rv); /* set the initial velocities */ /* * the KbdInput object allows nonblocking input */ KbdInput KI; char c; /* * the sense/act loop implementing reactive control */ KI.on(); /* turn on raw input/output */ int angle; // angle to the goal point int tempAngle; // This is the angle to make the robot // right angle with obstacle. int turned = 0; // How many turn after wallfollowing // start??? bool followAngle = false; // moving toward to the end. bool wallFollow = false; // Whether the robot is following the wall /* * When the robot has returned to the point at which it originally hit the * obstacle, this will set to true, then we can trace the shortest point to * the end after one eitire turn. */ bool endTurn = false; Point current(State[34], State[35]); // robot's current position. Point followStart(0, 0); // The point where the robot hit the // obstacle. Point shortPoint(0, 0); // Shortest point to end point from the // obstacle. double shortLength = lengthIs(end, current); // Storage to store the shortest length to // the end point. double currentLength; // Temporary storage to store the current // length to the end point. while (1) { c = KI.nextChar(); /* c is 0 if there's no input ready */ if ( c == 'q' ) /* hit q in the terminal window to quit */ { break; } current = Point(State[34], State[35]); // Getting current potision. /* * If the robot is not following the wall, then check following * 1. If the robot is at the end position, we are done. * 2. If either x or y position hit the end position and the other * coodinator is VERY_CLOSE (close to 25 distance) to the end point, * get new angle to the end position and slow down and try to get the * exact end position. * 3. If both x and y are CLOSE (close to 50 distance), slow down to get * the end position. * * If the test fail all cases, then get the angle to the end position and * make the robot to foward to the end positon. */ if (!wallFollow) { /* * This is case of No. 1 */ if (current == end) // Yes, we are at the end position, end! { st(); ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[38] != 0) gs(); exit(1); } /* * This is case of No. 2 * * We are possibly close to the end position. get right angle to the * end position and slow down speed to get precise position. */ else if (current.getX() == end.getX() || current.getY() == end.getY()) { /* * Even if either x or y position is the end positon, * the can be far away if the other coordinator is not VERY_CLOSE. */ if ( abs(current.getX() - end.getX()) < VERY_CLOSE && abs(current.getY() - end.getY()) < VERY_CLOSE) { st(); // We are close to the end, let's get good // angle to the end position. ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[38] != 0) gs(); angle = angleIs(current, end); turnTo(LEFT, angle); tv = State[38] / 2; if (tv < VERY_CLOSE) tv = VERY_CLOSE / 2; vm (tv, 0, 0); } } /* * This is case of No. 3 * * We are CLOSE to the end position. Let's slow down to get precise * position. */ else if ( abs(current.getX() - end.getX()) < CLOSE && abs(current.getY() - end.getY()) < CLOSE) { tv = State[38] / 2; // Slow down the speed to the half. if (tv < CLOSE) tv = VERY_CLOSE; vm (tv, 0, 0); } /* * The robot is not close to the end. * Get the angle to the end position, and make the robot to foward to * the end positon. */ if (!followAngle) { angle = angleIs(current, end); turnTo(LEFT, angle); vm (tv, 0, 0); followAngle = true; } } /* * this tempAngle is the angle to make the robot to be perpendicular to * the obstacle. This angle will use only when the robot firstly hit the * obstacle. Otherwise, set this to the RIGHT_ANGLE (90 degree). */ if (followAngle) { tempAngle = RIGHT_ANGLE - angle; } else { tempAngle = RIGHT_ANGLE; } /* * If we found the wall, then, turn the robot to make the wall-follow. * The right angle is made previsely. */ if (turing(tempAngle)) { /* * This wall following program is right-wall-following (wall is at the * right side of the robot). So, if the robot hit the wall at the * left side, then the robot should make 180 degree turn so the * program execute right-wall-following. * This is only happened when the robot is getting to wallFollow */ if ((addUp(5, 9) < ADD_UP_MAX - 5) && followAngle) turnTo(RIGHT, RIGHT_ANGLE * 2); /* * remember the positon where the robot hit the obstacle. */ if (followAngle) { followStart = Point(State[34], State[35]); followAngle = false; } vm(tv, 0, 0); wallFollow = true; turned++; } int quater1 = addUp(13, 17); int quater2 = addUp( 1, 5); int quater3 = addUp( 5, 9); int quater4 = addUp( 9, 13); /* * If the robot is currenty wall-following, this will continuosly make * wall-following. */ if (wallFollow) { /* * If the robot is seeing only from the 4th quater(right bottom side), * whish is first case, or not seeing anything from right side, this * is the end of the wall, so, make the turn. * When obstacle is both side of the robot, the second case will * holde the case. */ if ( (quater1 + quater2 + quater3) == (ADD_UP_MAX * 3) && (quater4 < ADD_UP_MAX) || (quater1 + quater4) == (ADD_UP_MAX * 2) ) { /* * same as before, remember the positon whether the robot hit the * obstacle. */ if (followAngle) { followStart = Point(State[34], State[35]); followAngle = false; } /* * Wait until we get enough space to turn. */ while(addUp(9, 13) < (ADD_UP_MAX - 1)) { vm(tv, 0, 0); } turnTo(RIGHT, tempAngle); /* * When second case arisen, obstacle is both side of the robot, * then next time, the first case will arise, so the robot will * make an unnecessary turn. * This will prevent this unnecessary turn. */ while (addUp(13, 17) > (ADD_UP_MAX - MINIMUM)) { vm (tv, 0, 0); } wallFollow = true; // Anyway, we are wall-following. turned++; // Made one turn! } /* * After 2 turns (first is from entering wall-following and second is * from one turn after wall-following), check whether the robot has * returned to the point at which it originally hit the obstacle. If * so, we can follow the shortest length to the end point. */ if (turned > 2) { int diffX = abs(followStart.getX() - current.getX()); int diffY = abs(followStart.getY() - current.getY()); if ( diffX < LESS_CLOSE && diffY < LESS_CLOSE ) { endTurn = true; } } } /* * The robot has returned to the point at which it originally hit the * obstacle. Find the shortest position and make the robot to toward to * the end point. * If the robot hasn't returned, calculate the shortest length to the end * position. */ if (endTurn) { int diffX = abs(shortPoint.getX() - current.getX()); int diffY = abs(shortPoint.getY() - current.getY()); if ( diffX < LESS_CLOSE && diffY < LESS_CLOSE) { endTurn = false; wallFollow = false; turned = 0; st(); ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[38] != 0) gs(); } } else if (wallFollow) // Getting shortest length { currentLength = lengthIs(end, current); if (currentLength < shortLength) { shortLength = currentLength; shortPoint = current; } } gs(); /* update state */ } KI.off(); /* reset terminal settings */ disconnect_robot(1); /* gracefully disconnect from the robot */ } /* * lengthIs will get two points and return the length between two points */ double lengthIs(Point a, Point b) { int tempX = (b.getX() - a.getX()) * (b.getX() - a.getX()); int tempY = (b.getY() - a.getY()) * (b.getY() - a.getY()); double result = sqrt(tempX + tempY); return result; } /* * angleIs will get two points and return the angle to the point. */ double angleIs(Point from, Point to) { int steeringAngle = State[36]; int lengthB = to.getX() - from.getX(); int lengthC = to.getY() - from.getY(); /* * Result will be inverse tan of angle. */ double result = (90 - (atan2(lengthB, lengthC) * 180 / PI) ) * TENTH; /* * The robot could facing different angle. This will make the angle be * always start from 0 degree. */ result += ((steeringAngle < 1800) ? -steeringAngle : (3600 - steeringAngle)); return result; } /* * Function will make a 90 degree turn with appropriate input * * Function will firstly stop the robot and wait until it really stops. * and make a 90 degree turn, then go again. */ void turnTo(int side, int whatAngle) { vm(0, 0, 0); pr(0, whatAngle * side, whatAngle * side); ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[39] != 0) gs(); } /* * Function will check whether it is too close to the block or not. * sensors are 3 front sensors. * Why I put sensor 16 is, my robot will make left turn so the sensor 16 can * detect fastest than others. */ bool isClose() { if ( (State[1] < MINIMUM) || (State[2] < MINIMUM) || (State[15] < MINIMUM) || (State[16] < MINIMUM) ) return true; return false; } /* * Function will add up three front sensors and if it is less than certain * amount, 23, which is close to the wall, then it will make turn. * Even if this addup value is not less than 23, the robot can hit the wall: * when it is turning the edge, the value will be bigger than 23, so, we gotta * check whether there is a block or not. */ bool turing(int whatAngle) { int I_Add = State[1] + State[2] + State[16]; if (I_Add < 23 || isClose()) { turnTo(LEFT, whatAngle); return true; } return false; } /* * Function will add up sensors values * range will be determined by input. */ int addUp(int from, int to) { int result = 0; for (int i = from; i < (to + 1); i++) { result += State[(i == 17) ? 1 : i]; } return result; }