Pathfinding - BlueTooth - 4 NXTs - multiple queues

[tabbycatrobots]

My current project is dancing robots. I've taught my robots a simple dance routine, but they soon get out of step with each other.
I want to use BlueTooth to synchronize the robots. For over a month, I have been pathfinding, to try to understand how to
use BT. Pathfinding - I use the word pathfinding to mean - experimenting with, PLAYing WELL with, teaching oneself about a
particular subsystem of a robot, with the goal of finding an efficient, reliable method.

WARNING - messy code in attached files. It has anything that popped into my mind, that I may want to try for my next 2
projects, plus many statements to display intermediate results to the screen. There are also many currently unused variables, that I
used while debugging. I expect half this code will disappear as I merge this new BT code with my dancer code. But in some of the
"extra" code, there may be an idea that someone else can use.

Two reasons that I'm posting this pathfinding code are:
1. It has taken me over a month to build reliable, working BT code, and during that time I have posted some questions in the forums, and seen several others about BT. As my code is now working, I think there may be some ideas in it, that can help some one else.
2. And someone may find some bugs in my code, and I would appreciate hearing about these.

To simplify my BT code and help reliability, I've made several design decisions.
In my project, I designated one robot as the lead dancer (LDR in my code), and 3 as support dancers (sprt in my code).
I made the LDR the BT Primary, and the sprts as BT Secondaries. The sprts only communicate with the LDR. I think this
simplifies things somewhat compared to a design where every robot must communicate with every other robot. The LDR
sets the pace during the routine, so having the lead dancer as the BT Primary is a natural fit.
I have the LDR stop any program currently running on a sprt, and start a program on the sprt. This has 4 advantages
1. All dancers are running compatible programs.
2. All dancers are at a known point in their individual programs, at the same time.
3. The NXTs are sending / receiving compatible BT messages. (My opinion - don't underestimate the importance of this.)
4. It saves me some work - 12 fewer button pushes per test run. (It's good to have competent helpers, to whom one can delegate
tasks. Although it sometimes take a while to develop their competency.)
After the initial exchange of messages on queue 0, during setting up the BT connection, the sprts do not share queues
(mailboxes). Sprt 1 uses queues 1, 4, and 7; sprt 2 uses queues 2, 5, and 8, and sprt 3 uses queues 3, 6, and 9. At the
least, this lessened the chance of me making a coding error
All message exchanges will originate with a message from the LDR. Like we learned in second grade, the sprts should only respond
when recognized. The LDR will periodically poll each sprt. The sprt will send an ACK or NACK message (unsigned long), that may
contain data requested by the leader, or may contain a request from the sprt. The last 8 bits of every message is a serial number
for error detection. And there is some framework for error recovery, with the NACK, resend, and ignore_continue_from_here
messages. (See the file pathfind_BT_xx_globals.h for types of messages.)

After the initial BT is made and some canned messages are sent and checked, the connection is considered good. From
this point on, in my dance program, and the attached test programs, all messages will be unsigned long ints.
The code for the initial connections are in the files Ldr_BT_Init_xx.nxc and Sprt_BT_Init_xx.nxc. This may be the most
interesting code for many, based on some of the comments I've seen in the forums.
The BT connections are made by the call to sprt_init_bt_comms(), in main(), for the sprts. For LDR, in main(), a call to
ldr_duo_init_bt_comms() connects to 1 sprt. A call to ldr_quad_init_bt_comms() connects to 3 sprts.

N.B. In the file Sprt_BT_Init_xx.nxc, there are 2 versions of the function sprt_init_bt_comms().
One that has been cleaned up, and one that includes my "trials and errors". I left the second as there may be idea in it,
that I'll want someday for a future project. The first version will be used in my dancers. The second version, which is
currently commented out, displays much more data to the screen, which helps with initial debugging.

In the file wlrn_fs_bt_26_pathfind.nxc, is the function fs_init_bt_comms(), that turns off BT for a short time, then back on.
This forces connect retries from LDR, so I can test that code.

I tried 2 methods for passing multiple values back from a function,
snd_rcv_by_ref_remote_number(), similar to passing a pointer in ANSI C
snd_rcv_by_iom_remote_number(), uses SysIOMapWriteByID
Both are in the file Pathfind_BT_Lib_2012Feb08g.nxc.
One or the other may work better for a particular situation.

The 2 attached .zip files, that contain code to send various BT test messages.
The .zip file Tone_Test contains programs for trying to sync the NXTs so they all play a tone at the same time. This
will be modified in my dancer program, so all dancers take the next step at the same time.

The .zip file Various_Tests contains programs for sending and receiving various types of messages. Note though, that
all messages are encoded in an unsigned long. This set of programs also contains the test program that turns off BT.
To run this test, load both wlrn_fs_bt_26_pathfind.nxc and wlrn_lft_bt_26_pathfind.nxc on WLRN.

After the connect and initial exchange of messages to test the connection, the message "Push BTNctr" appears at the
bottom of the displays. To continue with the testing, push the orange button on ALL the sprt NXTS BEFORE pushing
the orange button on the LDR NXT.

I haven't tried running this code on a second set of NXTs, but I think all that is required is to change
#define NXT_1_NAME "WFCR"
#define NXT_8_NAME "NCPR"
#define NXT_4_NAME "WNYC"
#define NXT_9_NAME "WLRN"
in the file Pathfind_BT_xx_globals.h. Although it may reduce confusion to change the filenames to match your NXTs.

For those that aren't familiar with the tool WinDiff, I recommend it for help in writing and debugging files. It compares text
files. I use it when editing similar versions of the same file, e.g. the files for the 3 sprt dancers. It can be downloaded from
the Microsoft web site. It helps me to find lines that I need to edit, or to find typing mistakes.

If you use my code and remove some of the code to display results to the screen, the BT operations may stop working. BT ops take
a finite amount of time to complete. As has been mentioned in this forum before, if a second BT op is attempted before the
first completes, an error may occur. Displaying to the screen only takes a short time, but not zero time. If you take out some
display code, and see a error, try inserting a Wait(50); or Wait(100);. This may take some trial and error.

[afanofosc]

Thanks for posting this code. I will be looking over it in detail to see if I can understand exactly what you are doing.

John Hansen