function [] = mindstorm();

% Robot Controller for LEGO Mindstorm NXT
% G.Gutt 11/27/2005, Ver 1.0
% greg@mymailbox.org

% Uses Bluetooth serial port emulation/port 5, and RealTerm
% control for robust serial communications.   RealTerm controlled via
% ActiveX commands in matlab.  Get RealTerm here:http://realterm.sourceforge.net

% This program assumes that you have RealTerm installed and that you have a
% Bluetooth configured as a virtual serial com port (here I use port 5).

% (You can also use the Matlab native serial communication routines if you wish.
% However I have found to not be as robust as RealTerm.)

% See Mindstorm NXT Bluetooth Development documentation to understand the
% structure of Byte Strings.  Specifically:  Appendix 2-LEGO MINDSTORMS NXT Direct commands.pdf
% http://mindstorms.lego.com/Overview/NXTreme.aspx

% Other useful links and documentation
% Using RealTerm and Matlab: http://realterm.sourceforge.net/realterm_from_matlab.html
% Example of Bluetooth Remote for NXT written in C: http://www.norgesgade14.dk/legoSider/mindstorm_en.html

% Program should play "Ode to Joy" and Move motor 0 back and forth
% Developed Using Matlab 6.5 Release 13

clear;                                 %clear variables
close all;                             %close open figures
fclose('all');                         %close all ports/files
clc;                                   %clear the command window

% Global variable declarations
global ppwrstring;                     % hex string for
default powr - positive
global npwrstring;                     % hex string for
default powr - negative
global hrealterm;                      % activex handle for
realterm RS-232 terminal program

% Open and Configure RealTerm as a Server
disp('GMU Neural Engineering Lab');
disp('Mindstorm NXT Interface Test Program Ver 1.0');
disp('G.Gutt 11/27/2006');
disp(' ');

power = 65;                             % default power
level (65%) for the nxt motors       
ppwrstring=['0x', dec2hex(75)];         % pos pwr, convert
default power level to hex string 0x00 format
npwrstring=['0x', dec2hex(power +128)]; % negative powr string
realtermstart;                          % start realterm
under ActiveX for RS-232 control
song;                                   % Plays Ode to joy
testmotor;                              % reset all the
motors to absolute positioning 

disp('');
disp('Type "return" to finish program and close down
RealTerm');  
keyboard;                               % Keeps RealTerm and
Matlab active for debugging
invoke(hrealterm,'close');              % Close realterm down
delete(hrealterm);                      % Delete realterm handle

% Main Program Ends Here
******************************************************

% Sub Functions start here
%******************************************************************************
function [] = twait(t); 
  % twait(t)
  % simple routine to wait is in units of 100ms.  
  t=t/10; 
  tic
    while(toc<t)
    end;    % end of twait function
%******************************************************************************

%******************************************************************************
function [] = realtermstart();
  % setup real term for active x control of serial port, com5
  global hrealterm;   % activex handle for realterm RS-232
terminal program

  hrealterm=actxserver('realterm.realtermintf'); % start
Realterm as a server
  hrealterm.baud=9600;
  hrealterm.caption='Matlab Realterm Server';
  hrealterm.windowstate=0; %minimized
  hrealterm.Port='5'; %select port 5
  hrealterm.PortOpen=1; %open the comm port
  hrealterm.HalfDuplex=1;
  hrealterm.FlowControl=0;
  hrealterm.LinefeedIsNewline=0;
  hrealterm.DisplayAs=2;
%******************************************************************************

%******************************************************************************
function []= testmotor();
  global ppwrstring;   % motor power string in 0x00 Hex format
  global npwrstring;   % neg power string

  % resets the tachometer of motor to absolute positioning,
motor 0 
  %(5th byte in string sets motor to reset: 0,1,2)

  % sends command to turn motor 0 forward 25 degrees, pause,
then reverse
  sendbytes(['0x0D 0x00 0x80 0x04 0x00 ', ppwrstring, ' 0x07
0x01 0x50 0x10 0x19 0x00 0x00 0x00 0x00']);
  twait(20);  % wait 200msec)
  sendbytes(['0x0D 0x00 0x80 0x04 0x00 ', npwrstring, ' 0x07
0x01 0x50 0x10 0x19 0x00 0x00 0x00 0x00']);
  twait(20);  
  sendbytes(['0x0D 0x00 0x80 0x04 0x00 0x00 0x00 0x00 0x50
0x00 0x00 0x00 0x00 0x00 0x00']); % turn off motor
  
  % Breif byte structure for SetOutputState, NXT command to
move the motor
  % Byte 1, number of bytes in the message, message starts
on 3rd byte
  % Byte 2, 0x00 format byte, never changes
  % Byte 3, Begining of message, 0x80 format byte, never changes
  % Byte 4, 0x04, the actual command, never changes
  % Byte 5, Motor number 0x00, 0x01 or 0x02
  % Byte 6, Power level, -100 to 100 
  % Byte 7, Mode bit field, see Mindstorm Bluetooth documetation
  % Byte 8, Regulation Mode, see Mindstorm Bluetooth
documetation
  % Byte 9, Turn Ratio, -100 to 100
  % Byte 10, Run State, see Mindstorm Bluetooth documetation
  % Byte 11-15, Tacholimit, number of degrees to turn (all
zeros means run forever)
  
  
%******************************************************************************

%******************************************************************************
  function [] = tone(freq,duration);
  % plays a tone.  freq is in units of Hz, duration is in
units of 100ms

  fstring=dec2hex(freq);       % convert to hex
  padzeros=4-length(fstring);  % number of zeros needed to
pad hex output
  for i=1:padzeros
     fstring=['0',fstring];
  end;
  fstring=[' 0x',fstring(3:4),' 0x',fstring(1:2)];  %formats
frequency in 2 byte string 0x00 0x00
                                                    %note
bytes are reversed to form Uword
  tstring=dec2hex(duration*10);   % convert to hex
  padzeros=4-length(tstring);     % number of zeros needed
to pad hex output
  for i=1:padzeros
      tstring=['0',tstring];
  end;
  tstring=[' 0x',tstring(3:4),' 0x',tstring(1:2)];  %formats
tune in 2 byte string 0x00 0x00
                                                    %note
bytes are reversed to form Uword
  sendbytes(['0x06 0x00 0x80 0x03',fstring, tstring]);
  
  % Breif byte structure for PlayTone, NXT command to play a
tone
  % Byte 1, number of bytes in the message, message starts
on 3rd byte
  % Byte 2, 0x00 format byte, never changes
  % Byte 3, Begining of message, 0x80 format byte, never changes
  % Byte 4, 0x03, the actual command, never changes
  % Byte 5-6, Frequency of the tone in Hz, Uword
  % Byte 7-8, Duration of the tone in ms, Uword  
%******************************************************************************

%******************************************************************************
                                            
function [] = sendbytes(sendstring);
  % Converts String of HEX Numbers in 0x00 form to
Characters and sends
  % them out via Realterm

  global hrealterm;   % activex handle for realterm RS-232
terminal program

  byteparse=findstr(sendstring,'0x');  %finds the hex
notation start characters for the string
  outstring='';
  for i=1:length(byteparse);
      bytestring=sendstring(byteparse(i)+2:byteparse(i)+3);
      outchar=char(hex2dec(bytestring));
      outstring=[outstring, outchar];
      invoke(hrealterm,'PutChar',outchar); % send the
characters byte by byte
  end;
  disp(['Bytes Sent: ', sendstring]);
%******************************************************************************

%******************************************************************************
function [] = song();
  % format (freq - Hz, Duration units of 100 ms)
  % delay in units of 100ms
  % Song: "Ode to Joy"

  tone(440,4); twait(4);    
  tone(440,4); twait(4);
  tone(494,4); twait(4);
  tone(523,4); twait(4);
  tone(523,4); twait(4);
  tone(494,4); twait(4);
  tone(440,4); twait(4);
  tone(392,4); twait(4);
  tone(349,4); twait(4);
  tone(349,4); twait(4);
  tone(392,4); twait(4);
  tone(440,4); twait(4);
  tone(440,6); twait(6);
  tone(392,3); twait(3);
  tone(392,3); twait(6);
  tone(440,4); twait(4);
  tone(440,4); twait(4);
  tone(494,4); twait(4);
  tone(523,4); twait(4);
  tone(523,4); twait(4);
  tone(494,4); twait(4);
  tone(440,4); twait(4);
  tone(392,4); twait(4);
  tone(349,4); twait(4);
  tone(349,4); twait(4);
  tone(392,4); twait(4);
  tone(440,4); twait(4);
  tone(392,6); twait(6);
  tone(349,3); twait(3);
  tone(349,3);     
%******************************************************************************