NXT Shield Version 2
Update
The NXT Shield is for sale in our shop: http://shop.tkjelectronics.dk/product_info.php?products_id=29. A easy to use library is also provided: https://github.com/TKJElectronics/NXTShield.
Three examples that demonstrates reading the encoders, turning the motors and using the ultrasonic sensor is found in the library as well: https://github.com/TKJElectronics/NXTShield/tree/master/examples.
More pictures of the NXT Shield can be found at the following blog post: http://blog.tkjelectronics.dk/2012/04/nxt-shield-library/.
Instead of using the shield with an Arduino, one of our customers decided to hook it up to an STM32 processor to control his robot. He described his development and findings on his blog: https://www.embedded.com/design/programming-languages-and-tools/4458801/2/Making-robots-with-Ada—Part-2—Driving-the-motors
Please note however that his code is written in the Ada language and not C/C++!
I finally made a new version of my NXT Shield. The big news is that it now supports the Lego Ultrasonic Sensor and it has NXT Compatible Sockets from mindsensors.
I won’t go much in detail on how the motors and encoders work, as I did that in the old post. So if you want to know how they work. Se my previous post. Instead I will focus on the Lego Ultrasonic Sensor.
Ultrasonic Sensor
Below is the pinout for the Ultrasonic Sensor:
NXT Input Port
As there is already pullup resistors inside the Lego Ultrasonic Sensor (see the schematic for more details), there is no need to add those on the data and clock lines.
The I2C frequency has to be adjusted – the datasheet says 9600Hz, but I have adjusted it to 11494,253Hz, as it is the same as the one used by the NXT (measured with my logic analyzer).
For more information see this pdf, at “Input ports” and “I2C Communication”.
Furthermore if you have a look at the Appendix 7-LEGO MINDSTORMS NXT Ultrasonic Sensor I2C communication protocol, it says byte 2 is R + 0x03, this means that the user have to send a repeated start and read from the device straight away.
So for example if you want to read the product version. The official Arduino Library (Wire.h) doesn’t support repeated start. So instead I decided to use a modified version of Peter Fleury excellent library: I2Cmaster, as it can be used for any AVR devices.
The library is modified to work with the Lego Ultrasonic Sensor (I2C frequency set to 11494,253Hz), and the CPU frequency has been set to the one used by Arduino (16MHz). Keywords has also been added, so the commands will colored orange.
The modified library can be found here: i2cmaster library.
But that’s not all! For some reason Lego decided to use a non-offical I2C firmware, therefore the clock line needs an extra wiggle between the write and read commands as explained at this post. Therefore the digital pin 4 (D4) on the Arduino is connected to the clock line (SCL) on the shield.
Below is a example sketch which will print out the following on the terminal:
The first one is the version number (V1.0), the second one the product ID (LEGO), then the sensor type (Sonar), then measurements units (10E-2m). After that it starts printing out the distance measured by the sensor.
All the I2C commands supported by the Ultrasonic can be found here:
Appendix 7-LEGO MINDSTORMS NXT Ultrasonic Sensor I2C communication protocol.
byte clockPin = 4;
byte buf[9];//Buffer to store the received valeus
byte addr = 0x02;//address 0x02 in a 8-bit context - 0x01 in a 7-bit context
byte distance;
void setup()
{
i2c_init();//I2C frequency = 11494,253Hz
Serial.begin(115200);
printUltrasonicCommand(0x00);//Read Version
printUltrasonicCommand(0x08);//Read Product ID
printUltrasonicCommand(0x10);//Read Sensor Type
printUltrasonicCommand(0x14);//Read Measurement Units
}
void loop()
{
// printUltrasonicCommand(0x42);//Read Measurement Byte 0
distance = readDistance();
if(distance == 0xFF)
Serial.println("Error Reading Distance");
else
Serial.println(distance, DEC);
}
byte readDistance()
{
delay(100);//There has to be a delay between commands
byte cmd = 0x42;//Read Measurement Byte 0
pinMode(clockPin, INPUT);//Needed for writing to work
digitalWrite(clockPin, HIGH);
if(i2c_start(addr+I2C_WRITE))//Check if there is an error
{
Serial.println("ERROR i2c_start");
i2c_stop();
return 0xFF;
}
if(i2c_write(cmd))//Check if there is an error
{
Serial.println("ERROR i2c_write");
i2c_stop();
return 0xFF;
}
i2c_stop();
delayMicroseconds(60);//Needed for receiving to work
pinMode(clockPin, OUTPUT);
digitalWrite(clockPin, LOW);
delayMicroseconds(34);
pinMode(clockPin, INPUT);
digitalWrite(clockPin, HIGH);
delayMicroseconds(60);
if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
{
Serial.println("ERROR i2c_rep_start");
i2c_stop();
return 0xFF;
}
for(int i = 0; i < 8; i++)
buf[i] = i2c_readAck();
buf[8] = i2c_readNak();
i2c_stop();
return buf[0];
}
void printUltrasonicCommand(byte cmd)
{
delay(100);//There has to be a delay between commands
pinMode(clockPin, INPUT);//Needed for writing to work
digitalWrite(clockPin, HIGH);
if(i2c_start(addr+I2C_WRITE))//Check if there is an error
{
Serial.println("ERROR i2c_start");
i2c_stop();
return;
}
if(i2c_write(cmd))//Check if there is an error
{
Serial.println("ERROR i2c_write");
i2c_stop();
return;
}
i2c_stop();
delayMicroseconds(60);//Needed for receiving to work
pinMode(clockPin, OUTPUT);
digitalWrite(clockPin, LOW);
delayMicroseconds(34);
pinMode(clockPin, INPUT);
digitalWrite(clockPin, HIGH);
delayMicroseconds(60);
if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
{
Serial.println("ERROR i2c_rep_start");
i2c_stop();
return;
}
for(int i = 0; i < 8; i++)
buf[i] = i2c_readAck();
buf[8] = i2c_readNak();
i2c_stop();
if(cmd == 0x00 || cmd == 0x08 || cmd == 0x10 || cmd == 0x14)
{
for(int i = 0; i < 9; i++)
{
if(buf[i] != 0xFF && buf[i] != 0x00)
Serial.print(buf[i]);
else
break;
}
}
else
Serial.print(buf[0], DEC);
Serial.println("");
}
/*
' Wires on NXT jack plug.
' Wire colours may vary. Pin 1 is always end nearest latch.
' 1 White +9V
' 2 Black GND
' 3 Red GND
' 4 Green +5V
' 5 Yellow SCL - also connect clockpin to give a extra low impuls
' 6 Blue SDA
' Do not use i2c pullup resistor - already provided within sensor.
*/
NXT Shield
Below are some pictures of the shield, mounted on top of an Arduino, a FEZ Panda II, an overview of the soldered PCB, the bare PCB, and a picture of the top silkscreen.
NXT Shield mounted on top of an Arduino
NXT Shield mounted on top of a FEZ Panda II
NXT Shield PCB Soldered
NXT Shield PCB
NXT Shield PCB
NXT Shield top silkscreen
The schematic can be seen below:
NXT Shield schematic
The schematic can also be downloaded as PDF from here: NXT-Shield.pdf
Buy it as a kit
If you do not want to make the board yourself, then we are going to sell this shield to you as a kit in the beginning of 2012 from our store.
You are also free to make it yourself. All the gerber files ready for manufacturing at http://www.dorkbotpdx.org/wiki/pcb_order and schematic can be found here: NXT Shield Gerber files and PCB. The NXT Socket eagle part can be downloaded here.
Example code
An example code for controlling the motors can be seen below. I read and write directly to the registers in the interrupts, to reduce the amount of time. See this page for more information.
#define TachLeft1 2
#define TachLeft2 5
#define TachRight1 3
#define TachRight2 6
volatile int leftTachPos = 0;
volatile int rightTachPos = 0;
//Left motor
#define leftPWM 9
#define logicleft1 7
#define logicleft2 11
//Right motor
#define rightPWM 10
#define logicright1 12
#define logicright2 8
//Constatens for move functions
#define left 0
#define right 1
#define backward 0
#define forward 1
#define block 2
#define brake 1
#define coast 0
//Variabels for moving using encoders
int leftPos = 0;
int rightPos = 0;
boolean blockLeft;
boolean blockRight;
void setup(){
pinMode(TachLeft1, INPUT);
pinMode(TachLeft2, INPUT);
pinMode(TachRight1, INPUT);
pinMode(TachRight2, INPUT);
pinMode(leftPWM, OUTPUT);
pinMode(logicleft1, OUTPUT);
pinMode(logicleft2, OUTPUT);
pinMode(rightPWM, OUTPUT);
pinMode(logicright1, OUTPUT);
pinMode(logicright2, OUTPUT);
attachInterrupt(0, leftencoder, CHANGE); //pin 2
attachInterrupt(1, rigthencoder, CHANGE); //pin 3
Serial.begin(115200);
}
void loop(){
//Forward at full speed for 3 seconds
Serial.println("Forward for 3 seconds");
move(left, forward, 255);
move(right, forward, 255);
delay(3000);
//Backward at full speed for 3 seconds
Serial.println("Backward for 3 seconds");
move(left, backward, 255);
move(right, backward, 255);
delay(3000);
//Rotate 360 degrees (one resolution) and coast motors
Serial.println("Coast");
move(left,forward, 255, 360, coast);
delay(3000);
//Rotate 360 degrees (one resolution) and brake motors
Serial.println("Brake");
move(left,forward, 255, 360, brake);
delay(3000);
//Rotate 360 degrees (one resolution) and block motors
Serial.println("Block");
move(left,forward, 255, 360, block);
delay(3000);
}
void move(int motor, int way, int torque)
{
int pwm;
int logic1;
int logic2;
if(motor == left)
{
pwm = leftPWM;
logic1 = logicleft1;
logic2 = logicleft2;
}
else if(motor == right)
{
pwm = rightPWM;
logic1 = logicright1;
logic2 = logicright2;
}
analogWrite(pwm, torque);
if(way == backward)
{
digitalWrite(logic1, LOW);
digitalWrite(logic2, HIGH);
}
else if(way == forward)
{
digitalWrite(logic1, HIGH);
digitalWrite(logic2, LOW);
}
}
void move(int motor, int way, int torque, int rotation, int halt)
{
if(motor == left)
{
analogWrite(leftPWM, torque);
if(way == backward)
{
leftPos = leftTachPos-rotation;
digitalWrite(logicleft1, LOW);
digitalWrite(logicleft2, HIGH);
while(leftTachPos > leftPos);
}
else if(way == forward)
{
leftPos = leftTachPos+rotation;
digitalWrite(logicleft1, HIGH);
digitalWrite(logicleft2, LOW);
while(leftTachPos < leftPos);
}
}
else if(motor == right)
{
analogWrite(rightPWM, torque);
if(way == backward)
{
rightPos = rightTachPos-rotation;
digitalWrite(logicright1, LOW);
digitalWrite(logicright2, HIGH);
while(rightTachPos > rightPos);
}
else if(way == forward)
{
rightPos = rightTachPos+rotation;
digitalWrite(logicright1, HIGH);
digitalWrite(logicright2, LOW);
while(rightTachPos < rightPos);
}
}
if(halt == block)
{
stop(motor);
if(motor == left)
{
//digitalWrite(leftPWM, HIGH);
analogWrite(leftPWM, 100);//Might have to adjusted depending on the battery voltage
blockLeft = true;
}
else if(motor == right)
{
//digitalWrite(rightPWM, HIGH);
analogWrite(rightPWM, 100);//Might have to adjusted depending on the battery voltage
blockRight = true;
}
}
else if(halt == brake)
{
stop(motor);
if(motor == left)
blockLeft = false;
else if(motor == right)
blockRight = false;
}
else if(halt == coast)
{
if(motor == left)
{
digitalWrite(leftPWM, LOW);
blockLeft = false;
}
else if(motor == right)
{
digitalWrite(rightPWM, LOW);
blockRight = false;
}
}
}
void stop(int motor)
{
int pwm;
int logic1;
int logic2;
if(motor == left)
{
pwm = leftPWM;
logic1 = logicleft1;
logic2 = logicleft2;
}
else if(motor == right)
{
pwm = rightPWM;
logic1 = logicright1;
logic2 = logicright2;
}
digitalWrite(pwm, HIGH);
digitalWrite(logic1, HIGH);
digitalWrite(logic2, HIGH);
}
void leftencoder()
{
if((PIND & B00100100) == 0 || (PIND & B00100100) == 36)//pin 2 == pin 5
leftTachPos--;
else
leftTachPos++;
if(blockLeft)
{
if(leftTachPos < leftPos)
{
//Turn on logicleft1 (pin 7)
PORTD |= B10000000;
//Turn off logicleft2 (pin 11)
if((PORTB & B00001000) != 0)//Check if it's already off
PORTB ^= B00001000;//If not turn it off
}
else if (leftTachPos > leftPos)
{
//Turn on logicleft2 (pin 11)
PORTB |= B00001000;
//Turn off logicleft1 (pin 7)
if((PORTD & B10000000) != 0)//Check if it's already off
PORTD ^= B10000000;//If not turn it off
}
}
}
void rigthencoder()
{
if((PIND & B01001000) == 0 || (PIND & B01001000) == 72)//pin 3 == pin 6
rightTachPos--;
else
rightTachPos++;
if(blockRight)
{
if(rightTachPos < rightPos)
{
//Turn on logicright1 (pin 12)
PORTB |= B00010000;
//Turn off logicright2 (pin 8)
if((PORTB & B00000001) != 0)//Check if it's already off
PORTB ^= B00000001;//If not turn it off
}
else if (rightTachPos > rightPos)
{
//Turn on logicright2 (pin 8)
PORTB |= B00000001;
//Turn off logicright1 (pin 12)
if((PORTB & B00010000) != 0)//Check if it's already off
PORTB ^= B00010000;//If not turn it off
}
}
}
Features
To do
NB
The shield and code is licensed under a Creative Commons Attribution Noncommercial Share-Alike license.
I’m testing the ultrasonic sensor but Arduino always return :ERROR i2c_rep_start
What and where is wrong?
Thanks
@Jakub
Have you downloaded the modified library that I provided: http://blog.tkjelectronics.dk/wp-content/uploads/i2cmaster.zip ?
Yes I download it and copy “i2cmaster.h” , “keywords.txt” and “twimaster.cpp” to “/usr/share/arduino/libraries/i2c” (I use Linux Fedora 16)
I have Arduino Uno and I’m using Arduino 1.0 IDE. I use 2x Li-ion battery – 8,4V (instead of 9V).
(sorry for my bad english and thank you for answer!)
@Jakub
Okay, ti seems correct. Have you made your own shield or are you just using a breadboard? Have you remembered to connect pin 4 to the clockline?
Your grammar is not a problem, I’m not a native speaker either π
I’m using breadboard and i have connect 6pin of NXT conector to 4pin of arduino IO
Oh I’m noob! It shall be 5pin NXT to 4pin IO. Now it works
Very thanks for you for your advice, a library and build schemes
!!
@Jakub
Your welcome π
when i saw your wrting “NXT Sheld Vession2 , i want to ask you something.
By using an Arduino UNO ,i want to controll a NXT ultrasonic waves sensor
so i did download i2cmaster library from your website but
it didn’t work and it’s differnt from your result.
It changes into infinite loop on unsigned char i2c_start(unsigned char address)’s line
i don’t know the reason.
My H/W is using an Arduino UNO atmega328.
ps. if you tell me your e-mail address, i will send a screenshot.
@LeeJaeSeong
Hi.
Have you connected your Ultrasonic sensor properly to the Arduino I2C port? Have you also remembered to connect the extra clock wire?
Please write us an email at: mail@tkjelectronics.dk
Thomas
Hi,
I was reading through this, having made many futile attempts to learn about the I2C protocol, and used your code for the sensor. I managed to get the error messages down to one, after connecting NXT pin 5 to Arduino I/O 4: “Error reading distance”, but now I do not know how to progress from here!
Any help would be greatly appreciated!
@Nicolas Weninger
You should check out my newest post: http://blog.tkjelectronics.dk/2012/04/nxt-shield-library/ As I have created a library as well: https://github.com/TKJElectronics/NXTShield
Btw: you should connect pin 4, not pin 5. But you could of course just change the code to reflect your hardware.
You can also buy the NXT shield directly from our shop: http://shop.tkjelectronics.dk/product_info.php?products_id=29 if you don’t want to create it yourself.
Regards
Lauszus
@Lauszus
Thank you, but the problem still persists! (I am using a breadboard, but am planning to buy the shield).
Will it still work if I just cut the cable and put each wire on an arduino pin (for the ultrasonic sensor)? Or do I have to buy the shield?
@Elliot
Yes. I originally just breadboarded everything. But it was quite a mess, but it is possible π
But I would recommend buying the shield if you want to build a robot or so.
@Elliot
I tried cutting an NXT cable with the intent of making a breadboard compatible connector. My advice: don’t do it. Don’t waste your time, energy, or the cable. Stripping the black jacket is without damaging the wires inside is very difficult. Even if you succeed in removing the outer jacket, stripping the insulation off the internal wires without damaging the hair-thin stranded copper is very very difficult. And even if you succeed at all of that, working with or soldering the delicate stranded copper is very very very difficult.
And even IF you invested the time to overcome all of these obstacles, the end result is: you put a new connector on a cable. Not a very rewarding end result. Just buy this NXT shield or find a “Breadboard Connector Kit for NXT” for about $3.
This page states that “Encoder support on both motors Γ’?? 360 counts per revolution”. Does this mean that I can turn each motor with a precision of 1 degree?
@Arjen
Yes, and with the help of the library we wrote it’s actually really easy!
For instance this command will turn the motor forward 1 degree at full speed and then brake the motor:
For more information see the example sketch: https://github.com/TKJElectronics/NXTShield/blob/master/examples/Motors/Motors.ino
Thanks for the quick reply. Last question: after having a look at the code, I see that when make a motor move, the program waits until the motor is at the right position before the program continues:
if(way == backward)
{
leftPos = leftTachPos-rotation;
digitalWrite(logicleft1, LOW);
digitalWrite(logicleft2, HIGH);
while(leftTachPos > leftPos);
}
This indicates that you cannot activate motor 1 and motor 2 at the same time, but you have to wait until the first motor has reached the final position before you can activate the second motor. Is this correct? If so, is there a way to get around this: I’d like to turn both motors at the same time…
@Arjen
This has been fixed in the library. So you can simply call the function and then it is handled in the interrupt rutine instead. See: https://github.com/TKJElectronics/NXTShield/blob/master/NXTShield.cpp#L122-147
soo. . . what kind of diods are you using? thanks.
@Francisco
We are using 1N4148 diodes.
I am having problems. I connected 9V to pin 1, gnd to pins 2 and 3, 5V to pin 4, SCL and D4 to pin 5, SDA to pin 6, loaded your example code and the modified i2cmaster library, opened the serial terminal at 115200 baud and nothing was displayed. I’m at a complete loss as to what is wrong. I am using a sparkfun redboard, so basically an arduino uno, and have tested my ultrasonic sensor with the view function on my nxt to verify it works.
@mmarkle
You should not be using this code anymore, as there is now a library available: http://blog.tkjelectronics.dk/2012/04/nxt-shield-library/ which uses the standard Wire library by Arduino.
The library can be found here: https://github.com/TKJElectronics/NXTShield.
Thanks. Will this new code work with my breadboard setup?
@mmarkle
Yes it will. I did the first prototype on a breadboard.
I’ve connected it the same and uploaded the new code, but my problem remains the same. Nothing displayed on the serial monitor. Neither the tx or rx lights on my board are lighting up, leading me to believe there is no serial communication. Should I be connecting pin 5 to a different pin?
I also tried connecting ultrasound pin 6 to A4 and 5 to A5, as that is what the schematic indicates.
@mmarkle
You need to connect pin 4 to the SCL line as well.
Hi,
I’m using one of the kits and like it very much.
I am wondering if I am encountering expected behavior. I’ve measured approx. 8.5 volts from my battery, but with a PWM value of 5 volts, ie 100%, I only measure about 8.0 volts at the motor. Losing half a volt seems significant. How much loss should I expect to see?
Thanks,
Pat
@Pat Rogers
With the clarity that only come from pressing “send” I realize that the above is pilot error on my part. Please disregard.
@Pat Rogers
Are you sure that the voltage rail is not just dropping because of the increased load? Have you tried to measure both voltages at the same time preferably using an oscilloscope.
Hello,
I am using the NXT library for connecting ultrasonic to the Arduino mega 2560. I have connect all the NXT connector pins as described in schematic diagram. But i am receiving the 2 on Wire.endTransmission . What could be possible wrong.?