-- -- Mindsensors NRLink sample interface code -- Drive PF motors using the NRLink in PBlua -- Show how to control both motor channels simultaneously -- Upload new macros into the NRLink -- Dump the contents of the NRLink EEPROM to the console -- PWM speed control of the PF motors in pbLua -- -- Mark Crosbie 10 April 2009 -- mark@mastincrosbie.com -- http://www.mastincrosbie.com/Marks_LEGO_projects/LEGO_Projects.html -- -- This code would not have been possible without the I2C tutorial -- on the pbLua website: http://www.hempeldesigngroup.com/lego/pbLua/tutorial/pbLuaI2C.html -- -- In this code I assume that you have a differential drive robot powered by two PF -- motors. The IR receiver is connected to the motors and set to Channel 1 ----------------------------------------------------------------- -- Constants for talking to the NRLink NRLinkport = 3 -- change this depending on the port you connect the NRLink to NRLinkID = 0x02 -- Put the various constants into an NRLink table to avoid polluting the -- global namespace NRLink = {} NRLink.NRLinkDataBytes = 0x40 NRLink.NRLinkCommandReg = 0x41 NRLink.NRLinkReadResult = 0x42 NRLink.NRLinkWriteData = 0x42 NRLink.NRLinkDefault = 0x44 NRLink.NRLinkFlush = 0x46 NRLink.NRLinkHighSpeed = 0x48 NRLink.NRLinkLongRange = 0x4C NRLink.NRLinkShortRange = 0x53 NRLink.NRLinkSetADPAON = 0x4E NRLink.NRLinkSETADPAOFF = 0x4F NRLink.NRLinkTxUnassembled = 0x55 NRLink.NRLinkSelectRCX = 0x58 NRLink.NRLinkSelectTRAIN = 0x54 NRLink.NRLinkSelectPF = 0x50 NRLink.NRLinkMacroCommand = 0x52 NRLink.Macro_Short_range = 0x01 NRLink.Macro_Long_Range = 0x04 -- Macro commands to run macros stored in the NRLink EEPROM -- In each case we send the letter "R" (NRLink.NRLinkMacroCommand) )followed by the address -- of the macro in the EEPROM. The command is written into NRLink.NRLinkCommandReg - the -- NRLink Command register -- -- These addresses come from macros I have previously loaded into my NRLink NRLink.Ch1_A_Float = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x50 ) NRLink.Ch1_A_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x53 ) NRLink.Ch1_A_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x56 ) NRLink.Ch1_A_Brake = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x59 ) NRLink.Ch1_B_Float = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x5C ) NRLink.Ch1_B_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x5F ) NRLink.Ch1_B_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x62 ) NRLink.Ch1_B_Brake = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x65 ) NRLink.Ch4_A_Float = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x98 ) NRLink.Ch4_A_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x9B ) NRLink.Ch4_A_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0x9E ) NRLink.Ch4_A_Brake = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xA1 ) NRLink.Ch4_B_Float = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xA4 ) NRLink.Ch4_B_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xA7 ) NRLink.Ch4_B_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xAA ) NRLink.Ch4_B_Brake = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xAD ) -- Dual motor control macros - power both ports on the IR receiver NRLink.Ch1_A_Fwd_B_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xB0 ) NRLink.Ch1_A_Fwd_B_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xB3 ) NRLink.Ch1_A_Rev_B_Fwd = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xB6 ) NRLink.Ch1_A_Rev_B_Rev = string.char( 0x02, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, 0xB9 ) ------------------------------------------------------------------ -- Define some macros to load into the NRLink -- Macros are defined as -- Address, Number of bytes in macro, Macrobytes... NRLink.powerFunctionsMacros = { [1] = {0xB0, 0x02, 0x01, 0x50}, -- Motor Ch1 A Forw B Forw [2] = {0xB3, 0x02, 0x01, 0x90}, -- Motor Ch1 A Forw B Rev [3] = {0xB6, 0x02, 0x01, 0x60}, -- Motor Ch1 A Rev B Forw [4] = {0xB9, 0x02, 0x01, 0xa0}, -- Motor Ch1 A Rev B Rev [5] = {0xBC, 0x02, 0x11, 0x50}, -- Motor Ch2 A Forw B Forw [6] = {0xBF, 0x02, 0x11, 0x90}, -- Motor Ch2 A Forw B Rev [7] = {0xC2, 0x02, 0x11, 0x60}, -- Motor Ch2 A Rev B Forw [8] = {0xC5, 0x02, 0x11, 0xa0}, -- Motor Ch2 A Rev B Rev [9] = {0xC8, 0x02, 0x21, 0x50}, -- Motor Ch3 A Forw B Forw [10] = {0xCB, 0x02, 0x21, 0x90}, -- Motor Ch3 A Forw B Rev [11] = {0xCE, 0x02, 0x21, 0x60}, -- Motor Ch3 A Rev B Forw [12] = {0xD1, 0x02, 0x21, 0xa0}, -- Motor Ch3 A Rev B Rev [13] = {0xD4, 0x02, 0x31, 0x50}, -- Motor Ch4 A Forw B Forw [14] = {0xD7, 0x02, 0x31, 0x90}, -- Motor Ch4 A Forw B Rev [15] = {0xDA, 0x02, 0x31, 0x60}, -- Motor Ch4 A Rev B Forw [16] = {0xDD, 0x02, 0x31, 0xa0} -- Motor Ch4 A Rev B Rev } ------------------------------------------------------------------ -- setupI2C() - sets up the specified port to handle an I2C sensor -- From Ralph Hempel's tutorial function setupI2C(port) nxt.InputSetType(port,2) nxt.InputSetDir(port,1,1) nxt.InputSetState(port,1,1) nxt.I2CInitPins(port) end -- Initialise the NRLink for communication with the PF IR receiver function initNRLink(port, address) setupI2C(port) NRLinkCommand(port, address, NRLink.NRLinkFlush); NRLinkCommand(port, address, NRLink.NRLinkDefault); NRLinkCommand(port, address, NRLink.NRLinkLongRange); NRLinkCommand(port, address, NRLink.NRLinkSelectPF); end -- waitI2C() - sits in a tight loop until the I2C system goes idle -- From Ralph Hempel's I2C tutorial function waitI2C( port ) while( 0 ~= nxt.I2CGetStatus( port ) ) do end end -- Send a command byte to the NRLink function NRLinkCommand(port, address, command) local NRLinkMsg = string.char(address, NRLink.NRLinkCommandReg, command); waitI2C(port) nxt.I2CSendData(port, NRLinkMsg, 0) end -- Instruct the NRLink to run a macro at a given address function NRLinkRunMacro(port, address, macro) local NRLinkMsg = string.char(address, NRLink.NRLinkCommandReg, NRLink.NRLinkMacroCommand, macro) waitI2C(port) nxt.I2CSendData(port, NRLinkMsg, 0) end -- A delay function - will delay for the given number of milliseconds function delayms(count) local endTime = nxt.TimerRead() + count while(nxt.TimerRead() < endTime) do -- nothing end end -- Print the contents of the NRLink EEPROM onto the console. -- Each address is printed in a line of 8 characters as hex function dumpNRLinkEEPROM(port, address) local addr local bytes local s0 = string.char(0x02); print("--------------------------------------------------") for addr=0,255,8 do local cmd = s0 .. string.char(addr) nxt.I2CSendData(port, cmd, 8) waitI2C(port) bytes = nxt.I2CRecvData(port, 8) print(string.format("0x%2x: 0x%2x 0x%2x 0x%2x 0x%2x 0x%2x 0x%2x 0x%2x 0x%2x", addr, string.byte(bytes, 1), string.byte(bytes, 2), string.byte(bytes, 3), string.byte(bytes, 4), string.byte(bytes, 5), string.byte(bytes, 6), string.byte(bytes, 7), string.byte(bytes, 8) ) ) end print("--------------------------------------------------") end -- Install new macros into the NRLink -- You can modify the macro definitions to load whatever is convenient for your robot function installNRLinkMacros(port, address, macroBytes) local msg -- iterate over the byte array provided and send via the I2C link for i,v in ipairs(macroBytes) do msg = string.char(address) waitI2C(port) for j,byte in ipairs(v) do msg = msg .. string.char(byte) end nxt.I2CSendData(port, msg, 0) delayms(10) end end -- Stop both motors function brakeMotors(port) waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_A_Brake, 0) delayms(10) waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_B_Brake, 0) end -- Drive robot forward -- Note that the motors are mirrored relative to each other, so one must -- be driven forward, and the other backwards -- Drive for the specified number of milliseconds function driveForwards(port, duration) local start start = nxt.TimerRead() repeat waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_A_Fwd_B_Rev, 0) delayms(10) until( nxt.TimerRead() >= (start+duration) ) brakeMotors(port) end -- Drive robot backwards -- Note that the motors are mirrored relative to each other, so one must -- be driven forward, and the other backwards -- Drive for the specified number of milliseconds function driveBackwards(port, duration) local start start = nxt.TimerRead() repeat waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_A_Rev_B_Fwd, 0) delayms(10) until( nxt.TimerRead() >= (start+duration) ) brakeMotors(port) end -- Turn robot to the right -- This pivots the robot on the right wheel function turnRight(port, duration) local start start = nxt.TimerRead() repeat waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_A_Fwd, 0) delayms(10) until( nxt.TimerRead() >= (start+duration) ) brakeMotors(port) end -- Turn robot to the left -- This pivots the robot on the left wheel function turnLeft(port, duration) local start start = nxt.TimerRead() repeat waitI2C(port) nxt.I2CSendData(port, NRLink.Ch1_B_Rev, 0) delayms(10) until( nxt.TimerRead() >= (start+duration) ) brakeMotors(port) end initNRLink(NRLinkport, NRLinkID)