View unanswered posts | View active topics It is currently Thu Oct 23, 2014 5:26 am






Reply to topic  [ 3 posts ] 
Sample code for mindsensors NRLink-Nx to PF motors 
Author Message
Rookie

Joined: Wed Nov 14, 2007 1:44 pm
Posts: 10
Post Sample code for mindsensors NRLink-Nx to PF motors
Hi everyone,

I am trying to couple my NRLink-NX attached NXT to my Lego bulldozer. Mindsensors has some code samples on how to use the NRLink to manage an RCX but nothing so far on the PF Motors integration.

Has anyone here dones this before with success? I know it can be done but I just dont know how to code it.

Thanks & Best Regards,
KK


Fri Nov 16, 2007 3:01 am
Profile
Rookie

Joined: Wed Nov 14, 2007 1:44 pm
Posts: 10
Post 
Ok. I now have a working code base thanks to Dr. Nitin's assistance. I can't post file attachments here so I have to copy / paste the text. Apologies for formatting loss. Hope this helps others trying the same thing. Regards

/*
*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
*
* 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,

*/

//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 = S1; // 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;

byte replyMsg[7];

/////////////////////////////////////////////////////////////////////////////
//
// 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 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);
}


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

task main()
{

byte Data[10];

nI2CBytesReady[NRLinkPort] = 0;


SensorType[NRLinkPort] = sensorI2CCustomFast9V;
nxtDisplayTextLine(0,"mindsensors.com");
nxtDisplayTextLine(1,"NRLink");
NRLinkCommand(NRLinkFlush);
NRLinkCommand(NRLinkDefault);
NRLinkCommand(NRLinkLongRange);
NRLinkCommand(NRLinkSelectPF);

while(1)
{
NRLinkRunMacro(Motor_Ch1_A_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_A_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_A_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_A_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch1_B_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_B_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_B_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch1_B_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch2_A_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_A_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_A_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_A_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch2_B_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_B_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_B_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch2_B_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch3_A_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_A_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_A_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_A_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch3_B_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_B_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_B_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch3_B_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch4_A_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_A_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_A_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_A_Brake);
wait10Msec(20);

NRLinkRunMacro(Motor_Ch4_B_FWD);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_B_Float);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_B_REV);
wait10Msec(20);
NRLinkRunMacro(Motor_Ch4_B_Brake);
wait10Msec(20);

}
// NRLinkRunMacro(Macro_Power_off );
StopAllTasks();

}


Thu Nov 22, 2007 12:40 pm
Profile
Rookie

Joined: Sun Jul 22, 2007 6:32 pm
Posts: 16
Location: Ireland
Post Control two PF motors using NRLink-Nx
Hi,

I've tried the above code and it will work fine controlling just one PF motor, but if I try to power two motors at the same time then only one motor ever turns on. It seems that the command buffer on the NRLink is overwritten and one command is lost each time.

So say you had a differential drive robot with a motor for each wheel you could expect to turn on motor A and B and have them both power on. But this does not happen.

Has anyone had any luck with this?

Regards,
Mark


Sun Mar 30, 2008 5:49 pm
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 3 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.