Xref: lugnet.com lugnet.robotics:5784 Newsgroups: lugnet.robotics Path: lugnet.com!lugnet From: "Hao-yang Wang" X-Real-Life-Name: Hao-yang Wang Subject: Homing with the IR Tower Content-Type: text/plain; charset=iso-8859-1 Content-Transfer-Encoding: 8bit X-Nntp-Gateway: http://www.lugnet.com/news/post/ Organization: none Message-ID: X-Mailer: Mozilla/4.0 (compatible; MSIE 4.01; Windows NT) X-Nntp-Posting-Host: lugnet.com Date: Fri, 23 Jul 1999 03:13:48 GMT Lines: 408 A while ago I was intrigued by the soda can retrieval challenge. Particularly because at that time I had built two different designs of grip-and-lift claws: The first is based on the bar-code truck claw. It uses the flex system and lifts things overhead. Moreover I also add a shock absorber and an additional touch sensor to its base, so it also acts as a bumper. The second claw is based on that famous Recycler, but mine is driven directly by the motor and involves no pneumatics. Since then, however, the project has essentially stalled, for two reasons: 1) On March 23 our son, Linus, was born; 2) I bought Mindstorms for robotics, but then Mindstorms brought me out of my dark age, and now ironically I find myself spending most of my precious spare time play Lego, not Mindstorms. Still, I'd like to show you what little progress I have made, namely how the robot finds its way home, after it locates and grabs the soda can. In my plan, the IR tower sits in the center of the home base, surrounded by floor marks of one to two feet in radius. The IR signal guides the robot to the floor marks, then the robot can follow the floor marks to the exact point to drop the can. The IR tower is set to the far mode, while the RCX is set to the near mode. There is a 8-stud-long hood on the IR unit of the RCX, to increase its directionality. Whenever the RCX receives a message from the IR tower (either MSG_GOT_NOTHING=7 or MSG_GOT_PING=8), it replies with a message MSG_PING=5. The IR tower sends out a MSG_GOT_PING message when it receives MSG_PING from the RCX, otherwise the tower sends out MSG_GOT_NOTHING five times a second. From the RCX's point of view, it either 0) receives nothing from the tower; 1) receives MSG_GOT_NOTHING, indicating that the tower does not receive messages from the RCX; or 2) receives MSG_GOT_PING, indicating that it has established the two-way communication. In this way, we get three levels of signal strength. The robot navigates by heading toward the mid-point of the band of the strongest signal. The robot is a standard two-track design. It has two motors, each drives a track with a 1:3 gear reduction. It uses two rotation sensors as tachometers to maintain its heading. The rotation sensors are not connected to the motors, but to two unpowered wheels. This way the rotation sensors still detect the actual movement of the robot even when the tracks are slipping. The wheels are suspended in a way that maintains their contacts to the ground even on an uneven terrain. The IR tower is controlled by a small program running on PC. Since spirit.ocx does not provide the API for sending/receiving RCX messages through the IR tower, I have to resort to the low level serial protocol (as described in ) to send/receive the Set Message opcode. The program is written in Objective Caml (). In my room, the robot can always get itself within two feet of the IR tower, often within one foot, _if_ it is not obstructed by some obstacles or mislead into walls by reflected IR signals. I think the failures can be overcome by a better avoidance algorithm. I use the IR unit alone, with no additional sensors. It will be interesting to compare it with the hybrid approaches, for example the one described in . On benefit of using the IR unit instead of the light sensor is that the IR unit detects only pulses of certain frequencies. This filters out most background noises. However, the current firmware ignores messages with wrong check sums. For navigation this is undesirable. (A wrong signal is better than no signals at all.) We may get better results by switching to other firmware such as LegOS so we can have direct control to the IR unit. We can replace the IR tower with another RCX. The drawback is of course the cost. The benefits include: 1) It can be programmed in the same language as the robot. 2) When the robot is locating the cans using the light sensor or Dennis Clark's IRPD, a shining IR tower can bring unwanted interference. A RCX at the home base can start pinging only after the can is located. (The robot sets itself to the far mode, send messages to start pinging, then sets back to the near mode.) From here to a fully functional can-retriever, there are still issues to be resolved. Here are a few of them: 1) Can detection. I have an interesting idea of detecting soda cans using Dennis Clark's IRPD: The robot wanders around. When it detects something in the IRPD, it does a bit of wall-following. (See my earlier post .) From the readings of its two tachometers, the robot can deduce the shape of the "wall". If the "wall" is a cylinder with a diameter in a certain range, then it may be a can. 2) Number of sensors: Now we have two rotation sensors as tachometers, one light sensor to detect the floor marks, and one IRPD to detect the soda cans... Oops. None of these sensors can share the input port with each others. I plan to connect the floor mark detector and the can detector to a 2-way power splitter (See .) Depending on the claw position, only one of them is active. So IRPD is on when the robot is seeking for cans with its claw down, and the light sensor is on when the robot is bringing back the can with its claw up. (We may need additional touch sensors for the operation of the claw, but these touch sensors can share ports with the active sensors, in particular the robot does not move when it uses its claw.) 3) Alternatives to RCX. Maybe we should switch to something with more input ports. Maybe mini-board. If I use Cyber Master, I can free up the two ports taken up by the tachometers, but then I cannot use the IR unit, the light sensor, and IRPD. If I mount RCX on top of Cyber Master, how do I make them communicate with each others? (See, I am trying to find excuses to justify buying a set of Cyber Master.) 4) Bumper. Currently the robot has no bumpers. It uses the tachometers to detect obstacles. (If the motors are running, but the tachometers do not change, then the robots must have bumped into something.) This way an obstacle is detected only after the robot has crashed into it, which makes me quite uncomfortable. I may add bumper(s) into the design. It is a bit tricky to have the bumper coexist with the claw. 2) The claw should not obstruct the IR unit. I may put the claw on the other end of the robot. The NQC source code for the RCX is included at the end of this message. The binary executable for the PC is available upon request. Cheers, Hao-yang Wang // Homer.nqc -- a robot that homes in the IR tower // (c) 1999 Hao-yang Wang, hywang@pobox.com // two rotation sensors connected to two wheels, acting as tachometers #define IN_TACHO_LEFT IN_1 #define IN_TACHO_RIGHT IN_3 // The difference between the two tachometers indicate the heading of // the robot. #define CalcTachoDiff(var) \ { var = IN_TACHO_RIGHT; \ var -= IN_TACHO_LEFT; \ } // the tachometer difference that makes up a whole circle, and a bit more #define WHOLE_CIRCLE_TACHO_DIFF \ ( 16 * 24 /*distance between the two wheels*/ \ / 3 /*wheel radius*/ \ + 8 /*a little bit more*/) // The task Ping puts the current signal level in sig_level . // 0 means no messages from the IR tower; 1 means getting a MSG_GOT_NOTHING // message from the tower, indicating that the tower does not get our // ping-back; 2 means getting a MSG_GOT_PING message from the tower, // indicating that the tower also gets our ping-back. int sig_level; // The task Ping also puts into the following two variables the robot heading // when the last messages from the IR tower are received. int level1_tacho_diff, level2_tacho_diff; #define MSG_PING 5 #define MSG_GOT_NOTHING 7 #define MSG_GOT_PING 8 #define NOISE_THRESHOLD 2 #define MESSAGE_TIMER 0 #define PING_BACK_TIME 1 #define NACK_TIME 6 #define SIG_L0_NOTE 330 #define SIG_L1_NOTE 440 #define SIG_L2_NOTE 660 #define SIG_NOTE_DURATION 10 #define SetSigLevel(level) \ { sig_level = level; \ \ if (level == 0) \ PlayNote(SIG_L0_NOTE, SIG_NOTE_DURATION); \ else if (level == 1) \ PlayNote(SIG_L1_NOTE, SIG_NOTE_DURATION); \ else \ PlayNote(SIG_L2_NOTE, SIG_NOTE_DURATION); \ } task Ping { int nothing_count; int cur_tacho_diff; sig_level = 0; IRMode(IR_LO); while (true) { if (Message() == 0) { SetSigLevel(0); wait (Message() != 0); } CalcTachoDiff(cur_tacho_diff); if (Message() == MSG_GOT_PING) { level2_tacho_diff = cur_tacho_diff; if (sig_level != 2) SetSigLevel(2); nothing_count = 0; } else if (Message() == MSG_GOT_NOTHING) { level1_tacho_diff = cur_tacho_diff; if (sig_level < 1) SetSigLevel(1); nothing_count += 1; } ClearMessage(); ClearTimer(MESSAGE_TIMER); SendMessage(MSG_PING); if (sig_level > 1 && nothing_count >= NOISE_THRESHOLD - 1) { wait (Message() != 0 || Timer(MESSAGE_TIMER) >= PING_BACK_TIME); if (Message() != MSG_GOT_PING) SetSigLevel(1); } wait (Message() != 0 || Timer(MESSAGE_TIMER) >= NACK_TIME); } } // two motors, each powers a track #define OUT_LEFT OUT_A #define OUT_RIGHT OUT_C #define SPEED OUT_FULL #define BACKUP_SOUND 1 #define DEAD_SOUND 2 // These two variables keep track of the power of the motors. int speed_left, speed_right; #define LeftFwd(speed) { Rev(OUT_LEFT, speed); speed_left = (speed) + 1; } #define LeftRev(speed) { Fwd(OUT_LEFT, speed); speed_left = -(speed) - 1; } #define RightFwd(speed) { Rev(OUT_RIGHT, speed); speed_right = (speed) + 1; } #define RightRev(speed) { Fwd(OUT_RIGHT, speed); speed_right = -(speed) - 1; } inline LeftOff { speed_left = 0; Off (OUT_LEFT); } inline LeftFloat { speed_left = 0; Float(OUT_LEFT); } inline RightOff { speed_right = 0; Off (OUT_RIGHT); } inline RightFloat { speed_right = 0; Float(OUT_RIGHT); } inline GiveUp { Off(OUT_A+OUT_B+OUT_C); PlaySound(DEAD_SOUND); IRMode(IR_HI); StopAllTasks(); } int tacho_diff, new_tacho_diff; // When the robot has turned around a circle (starting from tacho_diff) // without detecting any changes in sig_level, it gives up. In a real // application it should instead wander to a different location and // try again. sub TurnAroundCheck { CalcTachoDiff(new_tacho_diff); new_tacho_diff -= tacho_diff; if ( new_tacho_diff >= WHOLE_CIRCLE_TACHO_DIFF || new_tacho_diff <= -WHOLE_CIRCLE_TACHO_DIFF) GiveUp(); } // This task walks the robot: It wanders around, searches for the soda can, // then bring the can back home. Currently it does only the homing part. task HaveFun { int prev_level, cur_level; // We have to copy sig_level into cur_level, because sig_level may be // changed by task Ping any time. while (true) { LeftRev(SPEED); RightFwd(SPEED); cur_level = sig_level; CalcTachoDiff(tacho_diff); do { TurnAroundCheck(); prev_level = cur_level; cur_level = sig_level; } while (prev_level >= cur_level); do { if (prev_level < cur_level) { if (cur_level == 1) tacho_diff = level1_tacho_diff; else tacho_diff = level2_tacho_diff; prev_level = cur_level; } TurnAroundCheck(); cur_level = sig_level; } while (prev_level <= cur_level); if (prev_level == 1) new_tacho_diff = level1_tacho_diff; else new_tacho_diff = level2_tacho_diff; LeftFwd(SPEED); RightRev(SPEED); tacho_diff += new_tacho_diff; // Head toward the mid-point. tacho_diff /= 2; // // See? We don't care whether the robot is left- or right-handed. CalcTachoDiff(new_tacho_diff); if (new_tacho_diff > tacho_diff) do { CalcTachoDiff(new_tacho_diff); } while (new_tacho_diff > tacho_diff); else while (new_tacho_diff < tacho_diff); { CalcTachoDiff(new_tacho_diff); } RightFwd(SPEED); cur_level = sig_level; if (cur_level > 0) do { prev_level = cur_level; cur_level = sig_level; } while (cur_level >= prev_level); } } #define STUCK_TIMER 1 #define STUCK_WAIT_TIME 4 #define BACKUP_TIME 20 task main { int tacho_left, tacho_right; Sensor(IN_TACHO_LEFT, IN_ANGLE); Sensor(IN_TACHO_RIGHT, IN_ANGLE); start Ping; start HaveFun; while (true) { ClearTimer(STUCK_TIMER); tacho_left = IN_TACHO_LEFT; tacho_right = IN_TACHO_RIGHT; while ( tacho_left == IN_TACHO_LEFT && tacho_right == IN_TACHO_RIGHT && (speed_left != 0 || speed_right != 0)) if (Timer(STUCK_TIMER) >= STUCK_WAIT_TIME) { // The tachometers do not change, while the motors are // running. The robot must have bumped into something: // Back up. stop HaveFun; // This piece of code needs some improvements: First, it is // time-based: We should get rid of BACKUP_TIME, and use // the tachometers to determine whether the robot has backed // up far enough. Second, it simply reverse the directions of // the motors: The robot may bumps into the same spot again. // Often a wall reflects the IR signal and misleads the robot // to bump into it. We'd better move the robot way from // the wall before we start HaveFun again. ClearTimer(STUCK_TIMER); Toggle(OUT_LEFT+OUT_RIGHT); speed_left *= -1; speed_right *= -1; PlaySound(BACKUP_SOUND); while ( tacho_left == IN_TACHO_LEFT && tacho_right == IN_TACHO_RIGHT) if (Timer(STUCK_TIMER) >= STUCK_WAIT_TIME) { // Still not moving: Give up. GiveUp(); } wait (Timer(STUCK_TIMER) >= BACKUP_TIME); start HaveFun; break; } } }