//*!!Sensor,    S1,               NRLINK, sensorI2CCustomStd9V,      ,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors NRLINK               = (tSensors) S1;   //sensorI2CCustomStd9V //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

/*
*This is sample program to use with mindsensors.com NRLink interface.
*V1.1
*
*
* Written by Dr. Nitin Patil
* Copyright (c) mindsensors.com 2006, 2007
* for more info visit www.mindsensors.com
*
* Modified by Mark Crosbie mark@mastincrosbie.com  1 April 2008
* To install macros that power both motors simultaneously
* For more information visit http://markclego.mastincrosbie.com
*
* This sample has been updated to demonstrate
* the use of NRLink to drive PF Motors
*
*/

/*

NXT I2C register storage

location Read contents Write contents
---------------------------------------------------------
0-7 v1.1 -
8-f mndsnsrs -
10-18 NRLink -
=========================================================

location Read contents Write contents
---------------------------------------------------------
0x40 bytes in input buffer bytex to transmit to RCX
0x41 Status Flag command
0x42 Rd Data Wr Data
0x50-0xff Rd/RW macro memory is located on reg 0x50 onwards
=========================================================


Command Meaning

F flush the buffers
L hi power (long Range) mode
S low power (short range) mode
D Default setup to 2400 Bps
H low power (short range) mode
U Tx unassembled raw data from macro
R 0xXX Run macro at address 0xXX
N ADPA on
O ADPA off
===================================================

Unchangeable (ROM) Macro Definitions
These macros are stored in ROM and hence not changeable. These
macros emulate the LEGO Remote Control functions. Please ignore
for the purposes of managing PF Motors from NXT using NRLink

Unchangeable macro definitions of NRLink

address definition RCX opcode
0x01 short range IR 0x31 0x00
0x04 long range Ir 0x31 0x01
0x07 power off 0x60
0x09 Run Program 1 0xD2 0x02 0x00
0x0D Run Program 2 0xD2 0x04 0x00
0x11 Run Program 3 0xD2 0x05 0x00
0x15 Run Program 4 0xD2 0x10 0x00
0x19 Run Program 5 0xD2 0x20 0x00
0x21 Stop all programs 0xD2 0x40 0x00
0x25 Motor A Fwd 0xD2 0x00 0x08
0x29 Motor A Rev 0xD2 0x00 0x40
0x2D Motor B Fwd 0xD2 0x00 0x10
0x31 Motor B Rev 0xD2 0x00 0x80
0x35 Motor C Fwd 0xD2 0x00 0x20
0x39 Motor C Rev 0xD2 0x00 0x01


Changeable (EEPROM) Macro Definitions
These macros are factory programmed into NRLink to manage RCX,
however these can be re-programmed. Please follow the procedure
listed below prior to attempting to run this code. The procedure
will load the PF Motor macros onto your NRLink and prepare it
for this mode of use.

The following procedure needs you to use NXT-G software from Lego
1 - Connect the NRLink to NXT and NXT to your computer using USB
2 - Open any new program and drag the NRLink 'Duck" block.
3 - Select correct port, Address(0x02) and action as "power Function Macro"
4 - You will see bottom line read NRlink V2.00
5 - Click the macro update checkbox,
on the right side of box write button and status will appear,
6 - Click on the write button and it will download the desired macros in NRLink,
once done button and status will disappear .

Now you are ready to test the macro, this point onwards you may use any
programing software like NBC, NQC, RobotC or NXTG


Changeable macro definitions of NRLink

address definition RCX opcode
0x50 Motor Ch1 A Float, 02, 01, 00,
0x53 Motor Ch1 A Forward, 02, 01, 10,
0x56 Motor Ch1 A Reversed, 02, 01, 20,
0x59 Motor Ch1 A Brake, 02, 01, 30,

0x5C Motor Ch1 B Float, 02, 01, 00,
0x5F Motor Ch1 B Forward, 02, 01, 40,
0x62 Motor Ch1 B Reversed, 02, 01, 80,
0x65 Motor Ch1 B Brake, 02, 01, c0,

0x68 Motor Ch2 A Float, 02, 11, 00,
0x6B Motor Ch2 A Forward, 02, 11, 10,
0x6E Motor Ch2 A Reversed, 02, 11, 20,
0x71 Motor Ch2 A Brake, 02, 11, 30,

0x74 Motor Ch2 B Float, 02, 11, 00,
0x77 Motor Ch2 B Forward, 02, 11, 40,
0x7A Motor Ch2 B Reversed, 02, 11, 80,
0x7D Motor Ch2 B Brake, 02, 11, c0,

0x80 Motor Ch3 A Float, 02, 21, 00,
0x83 Motor Ch3 A Forward, 02, 21, 10,
0x86 Motor Ch3 A Reversed, 02, 21, 20,
0x89 Motor Ch3 A Brake, 02, 21, 30,

0x8C Motor Ch3 B Float, 02, 21, 00,
0x8F Motor Ch3 B Forward, 02, 21, 40,
0x92 Motor Ch3 B Reversed, 02, 21, 80,
0x95 Motor Ch3 B Brake, 02, 21, c0,

0x98 Motor Ch4 A Float, 02, 31, 00,
0x9B Motor Ch4 A Forward, 02, 31, 10,
0x9E Motor Ch4 A Reversed, 02, 31, 20,
0xA1 Motor Ch4 A Brake, 02, 31, 30,

0xA4 Motor Ch4 B Float, 02, 31, 00,
0xA7 Motor Ch4 B Forward, 02, 31, 40,
0xAA Motor Ch4 B Reversed, 02, 31, 80,
0xAD Motor Ch4 B Brake, 02, 31, C0,

Motor Ch1 A Forw B Forw,     B0,  02,  01,  50,
Motor Ch1 A Forw B Rev,        B3,  02,  01,  90,
Motor Ch1 A Rev B Forw,        B6,  02,  01,  60,
Motor Ch1 A Rev B Rev,       B9,  02,  01,  a0,
Motor Ch2 A Forw B Forw,     BC,  02,  11,  50,
Motor Ch2 A Forw B Rev,        BF,  02,  11,  90,
Motor Ch2 A Rev B Forw,        C2,  02,  11,  60,
Motor Ch2 A Rev B Rev,       C5,  02,  11,  a0,
Motor Ch3 A Forw B Forw,     C8,  02,  21,  50,
Motor Ch3 A Forw B Rev,        CB,  02,  21,  90,
Motor Ch3 A Rev B Forw,        CE,  02,  21,  60,
Motor Ch3 A Rev B Rev,       D1,  02,  21,  a0,
Motor Ch4 A Forw B Forw,     D4,  02,  31,  50,
Motor Ch4 A Forw B Rev,        D7,  02,  31,  90,
Motor Ch4 A Rev B Forw,        DA,  02,  31,  60,
Motor Ch4 A Rev B Rev,       DD,  02,  31,  a0

*/

//definations for NRLink

const ubyte NRLinkID = 0x02;
const ubyte NRLinkDataBytes = 0x40;
const ubyte NRLinkCommandReg = 0x41;
const ubyte NRLinkReadResult = 0x42;
const ubyte NRLinkWriteData = 0x42;
const tSensors NRLinkPort = NRLINK; // Connect NRLink sensor to this port!!


const ubyte NRLinkDefault = 0x44;
const ubyte NRLinkFlush = 0x46;
const ubyte NRLinkHighSpeed = 0x48;
const ubyte NRLinkLongRange = 0x4C;
const ubyte NRLinkShortRange = 0x53;
const ubyte NRLinkSetADPAON = 0x4E;
const ubyte NRLinkSETADPAOFF = 0x4F;
const ubyte NRLinkTxUnassembled = 0x55;


const ubyte NRLinkSelectRCX = 0x58;
const ubyte NRLinkSelectTRAIN = 0x54;
const ubyte NRLinkSelectPF = 0x50;


const ubyte NRLinkMacro = 0x52;
const ubyte Macro_Short_range = 0x01;
const ubyte Macro_Long_Range = 0x04;


const ubyte Motor_Ch1_A_Float = 0x50;
const ubyte Motor_Ch1_A_FWD = 0x53;
const ubyte Motor_Ch1_A_REV = 0x56;
const ubyte Motor_Ch1_A_Brake = 0x59;

const ubyte Motor_Ch1_B_Float = 0x5C;
const ubyte Motor_Ch1_B_FWD = 0x5F;
const ubyte Motor_Ch1_B_REV = 0x62;
const ubyte Motor_Ch1_B_Brake = 0x65;

const ubyte Motor_Ch2_A_Float = 0x68;
const ubyte Motor_Ch2_A_FWD = 0x6B;
const ubyte Motor_Ch2_A_REV = 0x6E;
const ubyte Motor_Ch2_A_Brake = 0x71;

const ubyte Motor_Ch2_B_Float = 0x74;
const ubyte Motor_Ch2_B_FWD = 0x77;
const ubyte Motor_Ch2_B_REV = 0x7A;
const ubyte Motor_Ch2_B_Brake = 0x7D;

const ubyte Motor_Ch3_A_Float = 0x80;
const ubyte Motor_Ch3_A_FWD = 0x83;
const ubyte Motor_Ch3_A_REV = 0x86;
const ubyte Motor_Ch3_A_Brake = 0x89;

const ubyte Motor_Ch3_B_Float = 0x8C;
const ubyte Motor_Ch3_B_FWD = 0x8F;
const ubyte Motor_Ch3_B_REV = 0x92;
const ubyte Motor_Ch3_B_Brake = 0x95;

const ubyte Motor_Ch4_A_Float = 0x98;
const ubyte Motor_Ch4_A_FWD = 0x9B;
const ubyte Motor_Ch4_A_REV = 0x9E;
const ubyte Motor_Ch4_A_Brake = 0xA1;

const ubyte Motor_Ch4_B_Float = 0xA4;
const ubyte Motor_Ch4_B_FWD = 0xA7;
const ubyte Motor_Ch4_B_REV = 0xAA;
const ubyte Motor_Ch4_B_Brake = 0xAD;

const ubyte Motor_Ch1_A_Forw_B_Forw = 0xB0;
const ubyte Motor_Ch1_A_Forw_B_Rev = 0xB3;
const ubyte Motor_Ch1_A_Rev_B_Forw = 0xB6;
const ubyte Motor_Ch1_A_Rev_B_Rev = 0xB9;

const int kThreshold = 20; // threshold for sonar sensor


// extensions to the default macros loaded into the NRlink to
// allow for control of two motors simultaneously
const ubyte powerFunctionsMacros[] = {
  0xB0,  0x02,  0x01,  0x50, // Motor Ch1 A Forw B Forw
  0xB3,  0x02,  0x01,  0x90, // Motor Ch1 A Forw B Rev
  0xB6,  0x02,  0x01,  0x60, // Motor Ch1 A Rev B Forw
  0xB9,  0x02,  0x01,  0xa0, // Motor Ch1 A Rev B Rev
  0xBC,  0x02,  0x11,  0x50, // Motor Ch2 A Forw B Forw
  0xBF,  0x02,  0x11,  0x90, // Motor Ch2 A Forw B Rev
  0xC2,  0x02,  0x11,  0x60, // Motor Ch2 A Rev B Forw
  0xC5,  0x02,  0x11,  0xa0, // Motor Ch2 A Rev B Rev
  0xC8,  0x02,  0x21,  0x50, // Motor Ch3 A Forw B Forw
  0xCB,  0x02,  0x21,  0x90, // Motor Ch3 A Forw B Rev
  0xCE,  0x02,  0x21,  0x60, // Motor Ch3 A Rev B Forw
  0xD1,  0x02,  0x21,  0xa0, // Motor Ch3 A Rev B Rev
  0xD4,  0x02,  0x31,  0x50, // Motor Ch4 A Forw B Forw
  0xD7,  0x02,  0x31,  0x90, // Motor Ch4 A Forw B Rev
  0xDA,  0x02,  0x31,  0x60, // Motor Ch4 A Rev B Forw
  0xDD,  0x02,  0x31,  0xa0  // Motor Ch4 A Rev B Rev
};

/////////////////////////////////////////////////////////////////////////////
//
//     send Command to NrLink interface
//
/////////////////////////////////////////////////////////////////////////////
void NRLinkCommand(byte  NRLinkCommand)
{

   byte NRLinkMsg[5];
   const byte MsgSize         = 0;
   const byte Address         = 1;
   const byte CommandAddress  = 2;
   const byte Command         = 3;


   // Build the I2C message
   NRLinkMsg[MsgSize]        = 3;
   NRLinkMsg[Address]        = NRLinkID;
   NRLinkMsg[CommandAddress] = NRLinkCommandReg  ;
   NRLinkMsg[Command]        = NRLinkCommand;

   while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING)
   {
     // Wait for I2C bus to be ready
   }
   // when the I2C bus is ready, send the message you built
   sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0);
}

/////////////////////////////////////////////////////////////////////////////
//
// send macro and run it from NRLink interface
//
/////////////////////////////////////////////////////////////////////////////

void NRLinkRunMacro(byte NRLinkMacroAdd) {
  byte NRLinkMsg[5];
  const byte MsgSize = 0;
  const byte Address = 1;
  const byte CommandAddress = 2;
  const byte Command = 3;
  const byte MacroAddress = 4;

  // Build the I2C message
  NRLinkMsg[MsgSize] = 4;
  NRLinkMsg[Address] = NRLinkID;
  NRLinkMsg[CommandAddress] = NRLinkCommandReg;
  NRLinkMsg[Command] = NRLinkMacro;
  NRLinkMsg[MacroAddress] = NRLinkMacroAdd;

  while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING) {
    // Wait for I2C bus to be ready
  }

  // when the I2C bus is ready, send the message you built
  sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0);
}


//////////////////////////////////////////////////////////////////////////////////////////
//
// Send a Message via I2C
//
// Sends an arbitrary 2-byte message over an I2C port. It would be easy to modify for
// messages of different length. Simply adjust the function parameters and the initialization
// of the 'nMsg' array.
//
// Usually when writing to device the reply length will be zero.
//
//////////////////////////////////////////////////////////////////////////////////////////

bool sendI2CMessage(tSensors nPortIndex, ubyte registerIndex, ubyte nByte1, ubyte nByte2, ubyte nByte3)
{
   const int kI2CAddress    = 0x02;    // You may want to make this a function parameter
   const ubyte nMsg[] =
   {
      2 + 3,               // This is length field for transmitted message.
      kI2CAddress,         // The I2C address of the device. Almost all devices use value '0x02'
      registerIndex,      // The internal register index within the sensor to start writing at.
      nByte1,
      nByte2,
      nByte3
   };

   // wait for I2C bus to be available
   while (nI2CStatus[nPortIndex] == STAT_COMM_PENDING) { wait1Msec(1); }

   sendI2CMsg(nPortIndex, nMsg[0], 0);
   return true;
}


//////////////////////////////////////////////////////////////////////////////////////////
//
// Install additional power function macros into the NRLink that allow for
// simultaneously control of both attached motors
//
/////////////////////////////////////////////////////////////////////////////////////////

void installPFMacros() {

   int i, numPFmacros;

   nxtDisplayTextLine(2, "Writing macro");

   numPFmacros = sizeof(powerFunctionsMacros);

   for(i=0; i < numPFmacros; i+=4) {
     // install macro into address
     sendI2CMessage(NRLinkPort,                // port the NRLink is connected to
                    powerFunctionsMacros[i],   // register address to write the macro into
                    powerFunctionsMacros[i+1], // number of bytes in the macro
                    powerFunctionsMacros[i+2], // macro command
                    powerFunctionsMacros[i+3]);// macro command
  }

  nxtDisplayTextLine(3, "Done");

  wait10Msec(100);
}

/////////////////////////////////////////////////////////////////////////////
//
// Run some commands and macro to control PF Motors using NRLink.
//
/////////////////////////////////////////////////////////////////////////////

task main()
{

  nI2CBytesReady[NRLinkPort] = 0;
  SensorType[NRLinkPort] = sensorI2CCustom9V;
  NRLinkCommand(NRLinkFlush);
  NRLinkCommand(NRLinkDefault);
  NRLinkCommand(NRLinkLongRange);
  NRLinkCommand(NRLinkSelectPF);

  //installPFMacros();

  nxtDisplayTextLine(4,  "Driving");
   NRLinkRunMacro(Motor_Ch1_A_Forw_B_Rev);

   wait10Msec(1000);
}