Bluetooth connections and serial handles

In this section we've got:

BT_OpenHandle

BT_CloseHandle

BT_CloseAllHandles

BT_SetDefaultHandle

BT_GetDefaultHandle

BT_MakeConfigFile

To communicate with the NXT via bluetooth, we have to use the SPP (serial port profile), which basically works like a virtual
serial port. This is why we can send and receive data from within MATLAB through the serial port commands.

To handle different connections or bluetooth adapters on different computers easily, a certain ini-file with settings for
the MATLAB functions must be present in the current directory (or one that can be found through path settings).

The ini-file format looks like this

[Bluetooth]

SerialPort=COM3
BaudRate=57600
DataBits=8

SendSendPause=10
SendReceivePause=30

TimeOut=2

The serial settings should be self explaining. Explanation of the send-pause-values will follow later on. The TimeOut parameter only has an effect when using Windows. It sets the period the bluetooth stack should wait when it is "missing data".
The MATLAB-internal default value of 10 causes annoying freezes in certain robot programs on certain computers (a direct cause
is not yet found). By setting 2 (the toolbox default value), one should get a fairly stable experience with very rare execution
pauses of 2 seconds. Smaller timeout values can lead to real packet loss which has not been examined yet.

To create a bluetooth configuration ini-file, a standard editor can be used. A more comfortable way is to use the GUI-guided
program:

BT_MakeConfigFile;

The following functions work under Windows as well as Linux, with one big exception: The returned handle will be a serial struct in Windows, but a simple scalar file handle in Linux. All the internal functions take care of this, but if you should
want to access fields like .BytesAvailable of the handle, you can only do so in Windows, i.e. you must check the operating system using if ispc to avoid errors in Linux.

Now first have a look how to obtain a handle to a bluetooth connection.

% Before we open a handle, we clean up to avoid errors:
BT_CloseAllHandles('bluetooth.ini');
% This only closes all open serial ports matching the COM-port from the% ini-file. More drastical is to close all open COM-ports like this:
BT_CloseAllHandles;

Now we can open a connection. Make sure the bluetooth dongle is connected to the NXT brick (using the according software or
scripts) before calling this.

h = BT_OpenHandle('bluetooth.ini', 'check');

The optional argument 'check' causes the function so send a keep-alive-packet and to wait for the answer before returning a valid handle. This is very
comfortable as it detects a malfunctioning / closed bluetooth connection before the execution of other program code.

Set the global default handle, so that later on, whenever we're calling functions, we don't have to pass the handle every
time.

where ReplyMode either has to be 'reply' or 'dontreply', specifying wether we want an answer from the NXT or not. This command
essentially creates the binary data for a packet, taking care of payload size and similar things. For more details see inside
the "Bluetooth Engine Demo" file.

Now it's getting interesting. We've got two functions to send and receive data respectively. Because the LEGO NXT brick has
a 30ms latency when switching from transmit to receive mode, we can expect a 60ms latency for a whole sensor reading request.

Very important is that the NXT can apparently lose packets / commands, because the input buffer (or queue) is of limited size.
As we do not know any more details about this, the send and receive functions have the option to wait between subsequent send
operations (i.e. to be less "aggressive"). This is where the earlier mentioned settings from the ini file come in:

SendSendPause=10
SendReceivePause=30

In this case we demand a 10ms delay between two consecutive send operations. On the other hand, a 30ms pause is required between
each send and receive operation (and vice versa receive and send). This should give the NXT enough time so switch between
bluetooth transmission modes without loosing any packets.

Note: The functions are "intelligent" and only pause execution if it is necessary. So if you only try to send a packet once
every second, you will not notice this automatic delay, as it is not required.

% for this function, we always have to specify a valid serial port handle
BT_SendPacket(packet, handle);

Receiving packets is as easy. Make sure you have requested one before you try to collect something.

[type cmd statusbyte content] = BT_CollectPacket(handle);

The statusbyte will be checked automatically by this function, and if it contains an error message, an according warning will
be issued. You can disable the automatic status byte check by calling BT_CollectPacket(handle, 'dontcheck'). There is really just one special situation where this is needed: NXT_LSGetStatus (see documentation and function code).

BT_CollectPacket exactly retrieves one packet from the internal receive buffer. It does so by checking the length of the packet (first two
bytes) and then only reads the amount of data that belongs to this specific packet. Be very careful though: If you call it
without previously requesting data, there will be nothing to collect, hence the function will return nothing after a timeout
or crash, depending on your bluetooth adapter. Even worse, under Linux it will block without the possibility to break until
you physically turn off the bluetooth device.

NXT system commands

In this section we've got:

NXT_SetBrickName

NXT_SendKeepAlive

NXT_GetBatteryLevel

All functions beginning with NXT_ are basically just ported from the official LEGO NXT Bluetooth Protocol and Direct Commands documentation.

Specify a name up to 15 chars long (preferrably no spaces) to name your brick. This can be read out by bluetooth adapters
(and later on in a MATLAB function as well, NXT_GetDeviceInfo, once it will be implemented). This name will also show up in your bluetooth adapter software when browsing for devices.

NXT_SetBrickName('RaceRobotV3');

To keep the NXT from turning off automatically, send a keep-alive packet from time to time (if needed):

NXT_SendKeepAlive('dontreply');

If you're interested in the internal sleep time limit setting, just request it:

[status SleepTimeLimit] = NXT_SendKeepAlive('reply');
% To see after how many minutes the NXT will shut down:
minutes = SleepTimeLimit / 1000 / 60;

Every function that retrieves (i.e. "gets") values from the NXT is internally split into two parts. But this doesn't have
to concern you right now. Note how we called the functions without passing a blutooth handle - we simply set the default handle
at the beginning of our program or session.

NXT direct commands

These are the interesting ones:

NXT_PlayTone

NXT_SetInputMode

NXT_ResetInputScaledValue

NXT_GetInputValues

NXT_ResetMotorPosition

NXT_SetOutputState

NXT_GetOutputState

NXT_StartProgram, NXT_StopProgram

NXT_LSWrite, NXT_LSGetStatus, NXT_LSRead

This section only covers the syntax, for details what the functions do please consider the official LEGO documentation or
the function help.

% frequency is in Hz, duration in ms
NXT_PlayTone(frequency, duration);

% the common way would be this
NXT_SetInputMode(InputPort, SensorTypeDesc, SensorModeDesc, 'dontreply');
% but if you want an acknowledgement (usually not needed), then use
status = NXT_SetInputMode(InputPort, SensorTypeDesc, SensorModeDesc, 'reply');
% note: the statusbyte will be automatically checked anyway (and a warning% issued if necessary), but you can still check the status now to handle the% consequences properly...

NXT_ResetInputScaledValue(port);
% in this function there is no way to request an acknowledgement...

To execute "real" Mindstorms programs on the NXT (i.e. programs that you created using the official LEGO software and stored
it locally on the NXT), you can call this function:

NXT_StartProgram('MyDemo.rxe');
% the file extension '.rxe' can be omitted, it will then be automatically added

Stopping the currently running program can be accomplished with

NXT_StopProgram();

There are three more NXT direct commands: NXT_LSGetStatus, NXT_LSWrite, NXT_LSRead. They all have one purpose: Address digital sensors that use the I²C-Protocol. An example is the ultrasonic sensor, and for
more documentation you might want to see the sourcecode of OpenUltrasonic and GetUltrasonic.

The idea is to send data (containing I²C payload) first with NXT_LSWrite. Then check the sensor status in a loop using NXT_LSGetStatus until it is ready, i.e. until the status byte reports no error and all requested bytes are available. Those bytes can then
be received using NXT_LSRead.

High level sensor control

In this section we've got:

OpenSwitch

GetSwitch

OpenSound

GetSound

OpenLight

GetLight

OpenUltrasonic

GetUltrasonic

CloseSensor

The functions mentioned above provide an easy and simple way to access all sensors. The ultrasonic sensor uses a digital interface
and has to be handled in a different way internally, but using these high level functions, you won't see a difference. The
only thing worth mentioning: GetUltrasonic is about 1.5 up to 2 times slower than the other Get* functions (because internally 3 or sometimes 4 packets are needed instead of just 2).

The Open* functions use NXT_SetInputMode, while Get* calls NXT_GetInputValues. CloseSensor is necessary to turn off a specific sensor, e.g. turn off the light sensor's red LED in active mode. This will save power.

You can use raw port numbers starting with 0 just as seen in the official Mindstorms documentation, or you can use the named
constants SENSOR_1 to SENSOR_4 for better readability. So let's open a few sensors:

Note that it is possible to use NXT_SetInputMode with a custom mode you like instead of Open*, and the Get-functions will still work (this is not true for the ultrasonic sensor, but it has totally different modes anyway). Internally
they just return the .NormalizedADVal value. This makes it possible to automatically count claps and still use the simple functions. Example:

% now replace OpenSound by this
NXT_SetInputMode(SENSOR_2, 'SOUND_DB', 'PERIODCOUNTERMODE', 'dontreply');
NXT_ResetInputScaledValue(SENSOR_2); % this is needed if you want to start with 0if GetSound(SENSOR_2) > 500 % GetSound still works!% hey, this is a loud atmosphere...end%if% we could do whatever we wanted here:
pause(10) % take a little 10s nap
data = NXT_GetInputValues(SENSOR_2);
ClapCount = data.ScaledVal;

That was easy. The NXT did the counting for us. For more details about sensor modes see the LEGO documentation, but in a few
words: A "transition" (TRANSITIONCNTMODE) is whenever the value (.NormalizedADVal) changes between the 45% threshold (45% of 1023 is 460). As "period" (PERIODCOUNTERMODE) counts when the value goes down from somewhere above 45%, and then changes back up. The "count" happens during the raising
part. To see what exactly is happening try the GUI_WatchAnalogSensor tool.

The main point I wanted to make: As you can see, GetSound / GetLight etc. can peacefully coexist with the NXT_ functions.

High level motor control

In this section we've got:

SetMotor

GetMotor

SetPower

SetAngleLimit

SetTurnRatio

SetRampMode

SpeedRegulation

SyncToMotor

SendMotorSettings

GetMotorSettings

StopMotor

ResetMotorAngle

WaitForMotor

SwitchLamp

These functions provide a very high level and simple way to access almost all motor features. The idea behind this is as follows:
First you specify the motor number (port) you want to work with. All successive commands then affect this current motor. Once
you are done setting everything you need, you submit these changes and issue a send command.

Let's just walk through this

SetMotor(0); % first motor is active now

This is a great opportunity to show that all of these functions accept also strings as input for numbers, i.e.:

SetMotor('0');

What's the point of this? Well, we now can use this fancy syntax:

SetMotor 0

Another option is to use symbolic constants for port numbers, that map to easy understandable port labels just as found on
the NXT:

SetMotor(MOTOR_B); % identical to SetMotor(1);

In the end it's your choice which way you use to address the ports, however we recommend the usage of constants MOTOR_A, MOTOR_B and MOTOR_C.

GetMotor just returns the motor you previously set. It can be useful when mixing NXT_ functions inbetween (they need a motor port as input):

NXT_ResetMotorPosition(GetMotor, true); % good style, works with current motor

Now the active motor is set, and we can specify all the details we want. When we're done (i.e. when the motor should finally
start spinning), we send.

To let the motor run forever (or until you change your mind and tell it something different), use an angle limit of 0:

SetMotor 0
SetPower 50
AngleLimit 0% run till the end of time (or battery, for that matter)
SendMotorSettings

If we want the motor to rotate at a constant speed as accurate as possible, we have to enable speed regulation. Like all the
other commands, this affects the currently set motor only and the settings will only be applied with the SendMotorSettings command.

SpeedRegulation on

One noteworthy effect of the internal speed regulation is, that the motor will increase the power if the desired rotational
speed cannot be met. Example: Consider you set a power of 20 without speed regulation, but your bot is so heavy that it does
not move (although you can still hear the motors are trying to). If you enable speed regulation, the motor will internally
increase its power until the wheels are moving, just like they would normally without a force acting against them. However,
using speed regulation with a power of 100 does NOT give you more power. 100 percent is all that you can get.

Note: SpeedRegulation seems to somehow deteriorate the motor's precision when targeting small distances (small values for
SetAngleLimit), so use with care.

For driving robots, the regulation mode "synchronization" is very interesting. It synchronizes two motors and they then act
as if they were connected through an axle, so that the robot can drive straight forward. This is implemented using

SetMotor 1
SyncToMotor 2
SetPower 50
SendMotorSettings

In the example above our bot will start driving forward (assuming motor 1 and 2 are the ones with wheels). From the point
on where you call SyncToMotor, all settings affect both synced motors (if one of them is set active of course). Even SendMotorSettings will internally send two packets to each of the motors, so that you can use them as if they were really connected.

Only with synced motors it is possible to drive curves or turn around:

The turn ratio command shifts power between synced motors. 100 means one motor is spinning forward and the other backwards,
resulting in the maximal rotational effect possible. 50 means one motor is running and the other is stopped, and with values
bewteen 1 and 49 you can get nice curves (both wheels spinning, but not at equal speed). Needless to say, 0 results in a straight
forward direction. You can use negative values for opposite directions (i.e. left and right turning, but that depends on the
robot model).

Ok, so now we want to turn off synchronisation

SyncToMotor off

Note that a motor can only be synchronized to another motor or speed regulated at a time! We have to manually turn off synchronization, before enabling speed regulation again, and vice
versa!

SyncToMotor off
SpeedRegulation on

Note that the function StopMotor also resets motor regulation and synchronization. This is by design, see documentation.

We've got one special setting left:

SetPower 100
SetAngleLimit 1080
SetRampMode up

This uses the motor's runstate RAMPUP, which basically accelerates smoothly from the old power to the new power. It's only valid when you specify an angle limit.
This is the rotational distance during which the motor will adjust the power to the new value.

To decelerate ("brake" smoothly) use

SetPower 0
SetAngleLimit 1080
SetRampMode down

or turn it off again (runstate RUNNING, "normal" movement), which is also the default setting:

SetRampMode off

Once we've seen all options for the SendMotorSettings, it's worth to know that you can also access all these options with one single line.

SendMotorSettings(MOTOR_A, 50, 360, 'off', MOTOR_B, 25, 'up')

This results in MOTOR_A being synced to MOTOR_B, both running with power 50, angle-limit 360 degrees, speed-regulation turned 'off', ramp-mode is 'up', and a turn-ratio
of 25. Again it's your choice which way you like better, but it is clearly recommended to use the long version, whose syntax is better understandable.

Ok fine. Now we've got a very handy command here:

StopMotor(GetMotor, 'off');

This will turn off power to the current motor, enabling the so called COAST mode, in which you can rotate the motor freely
as you like. Of course I could've also written StopMotor(0, 'off'), but I wanted to demonstrate a possible use of GetMotor again (avoid too much hardcoding of motor numbers, or of any values at all if possible).

The other option is

StopMotor(GetMotor, 'brake');

This will actively stop and hold the motor at its current position. Try moving it, it is harder than you think ;-). Works
like an emergency brake for driving robots. It consumes quite a lot of power, so you shouldn't leave your bot in this setting
for too much time.

And finally we've got this handy version:

StopMotor alloff% note the syntax, short for StopMotor('all', 'off')

The parameter all is special, not just because it is turning off (or braking) all motors, but because it does so at once. Internally only one packet is sent for all three motors, so you only have to expect one third of the lag (compared to sending three packets).
Also, you can turn off the motors almost synchronously, compared to one by one with the usual sending delay. It's a nice command
that can very often be found at the start or end of programs to ensure all motors are turned off and don't take any power...

So now we can control the motors, but how do we poll their rotation sensor? The answer is:

out = GetMotorSettings(MOTOR_C);
% so lets look into the results:
out.IsRunning % boolean, true if the motor "does something"
out.Power % current power
out.AngleLimit % current set angle limit, 0 means none set
out.TurnRatio % current turn ratio
out.SpeedRegulation % boolean, speed regulated or not?
out.SyncToMotor % the motor this one is synced to. -1 means not synced
out.TachoCount % internal, non-resettable rotation-counter (in degrees)
out.Angle % current motor position, resettable using% ResetMotorAngle(port);
out.MotorBrake % boolean, is electronic braking enabled?% (this has nothing to do with braking in a common% sense, just ignore it, should be turned on by% default anyway as it improves motor performance)

This should give us all information about the motor we need. Internally a NXT_GetOutputState is used of course. Note that the .Angle value is actually the NXT internal BlockTachoCount, which you can reset using ResetMotorAngle() or NXT_ResetMotorPosition(GetMotor, true) (i.e. what is called relative motor position in the official Mindstorms documentation).

ResetMotorAngle(MOTOR_A);

The .TachoCount on the other hand is much like your car's mileage counter. It gives the total amount of degrees the motor has rotated since
the NXT was turned on, and it is not resettable (it will however be resetted, when a new external program is started, which
you can enforce using NXT_StartProgram). You can also set it back to zero if you turn the motor back exactly the amount it was turned forward :-). Restarting the
NXT brick is a more comfortable way...

We have got yet one more very handy function for you. Suppose you've just set your motor to run a certain distance, i.e. using

SetMotor 0
SetPower 20
SetAngleLimit 360
SendMotorSettigns

Now power 20 is not very fast, so it will take some time for the motor until it has reached its angle limit. But how do you
know when it's done? You could create a loop and constantly call GetMotorSettings to check wether .IsRunning is set or not. And this is exactly what WaitForMotor does.

SetMotor 0
SetPower 20
SetAngleLimit 360
SendMotorSettigns
WaitForMotor(GetMotor) % you've seen me using GetMotor before...% if this WaitForMotor was not here, we would send the% PlaySound command DIRECTLY after starting the motor!% Try it for yourself. Basically our Matlab is "blocked" now and holds% execution, until the motor is done...
NXT_PlaySound(440, 200)

This will play a short beep on the NXT immediately after the motor has reached its angle limit. Of course that does not mean that the motor has stopped already: It's currently in coast mode and will keep turning a short while. That is why, the
following commands sequence is often used to try to stop at a certain position:

% ...
SendMotorSettigns
WaitForMotor(GetMotor)
StopMotor(GetMotor, 'brake') % actively hold still
pause(1) % do so for a short while% active braking has a high power consumption, so:
StopMotor(GetMotor, 'off') % release motor

WaitForMotor has one more parameter that you can use. Imagine what happens with this code snippet:

See it? You've created an endless loop. Matlab will block till the end of time (or battery in this case), as the motor will
not stop. Why should it, it has no angle limit set.

Something else can happen: You can set an angle limit, let's say a certain distance for a driving robot. But there is a wall,
the bot can't go any further, and again: Endless loop, because the motor will not reach its angle limit.

The solution for this is a built-in timeout. Try:

WaitForMotor(GetMotor, 10)

or use this syntax if you like it better:

WaitForMotor 010

This time the function will only block for a maximum of 10 seconds. If the motor is done earlier, fine. Then the function
will exit of course. But if it got stuck or is in an endless loop, after the timeout period (you can use fractions by the
way, like 5.7) WaitForMotor will continue execution. The price for avoiding this endless loop / Matlab block: When specifying a timeout, you cannot be
sure that the motor really has stopped, or wether it timed out. So take care of the consequences on your own (e.g. by reading
.Angle to see wether the specified angle limit was met, or by checking .IsRunning to see if the motor is still blocked)...

The motor ports can also be used for other actuators besides the NXT motors. Lamps can be connected, too. The following function
can be used to turn the lamp on and a short while later off.

SwitchLamp(MOTOR_C, 'on');
pause(1)
SwitchLamp(MOTOR_C, 'off');

It is also possible to use 'all' as port-number, just like you've seen for StopMotor:

SwitchLamp alloff

There is no secret behind SwitchLamp - it just sets power 100 for the specific output port. You could have used SendMotorSettings as well, but SwitchLamp is easier in this case and better to understand.

Lowlevel and helper functions

Actually there is not much to see in here, just mentioning:

tictic

toctoc

wordbytes2dec

dec2wordbytes

readFromIniFile

You certainly know the very nice and handy Matlab functions tic and toc. They have one big disadvantage though: If you use them deep down in your functions, and then in some toplevel functions
again that call those lower level functions, well: You start messing with tic and toc, and won't get useable results.

As you can see quickly, tictic and toctoc just take an index (or ID if you want to call it like that) as input, allowing several time-counters next to each other at
one time. So we eliminated one problem, but now we have to keep track of the tictic-IDs we use. If I use tictic(3) in my function that calls your function, but your function uses ID 3 as well, we've got the same problem as before. So be
careful. Probably best to use numbers above 100 in lowlevel functions, and stay away from this range in higher levels...

Very quickly, just to mention: We've got two functions that can help you converting numbers into the correct binary byte ordering
(considering the little-endian format: LSB first, that the NXT uses). If you want more information, look it up in the documentation...

List of global variables

In this section there is just a list of global variables that are being used by our functions. PLEASE DO NOT EDIT THEM! If available, use appropriate Get and Set functions.

As seen above, in use by textOut, modifying is allowed:

global DisableScreenOut
global EnableLogging
global Logfilename

Used by tictic and toctoc, do not access or edit!

global ticticStart; % array of vectors / "clocks", whatever

Bluetooth internals. Do NOT access directly!

global BT_DefaultHandle % has its own Get and Set functionsglobal BT_SendSendPause % read from ini fileglobal BT_SendReceivePause % read from ini fileglobal BT_LastSendTime % used by BT_Send and BT_Collectglobal BT_LastReceiveTime % used by BT_Send and BT_Collect

NXT / high level motor control internals. Do NOT access directly:

global NXTMOTOR_CurrentMotor % used by Get and SetMotorglobal NXTMOTOR_State % array of structs, used by almost everythingglobal NXTMOTOR_LeftWheel % used later on for robot control in high levelglobal NXTMOTOR_RightWheel % functions like TurnRobot...

Private functions

These functions remain in the private subfolder of the modules work directory. M-files in this folder are only visible from the upper directory abd should only
be called internally. The following functions are private because they are not supposed to be used, unless you are developing new NXT functions.

initializeGlobalMotorStateVar initializes the NXTMOTOR_State struct, only for the first time. Set default values here (like the BRAKE bit)

resetMotorRegulation is a little helper that sets a few values inside the NXTMOTOR_State struct sometimes when synchronisation mode is changed. Improves code readability...