Mindstorms Ultrasonic Sensor Help - Solved!!

Update
I got it working. For more information see my blog: TKJ Electronics » NXT Shield Version 2.

Hello everybody
I have tried for a while to talk to my ultrasonic sensor from my lego mindstorms kit, but I can not get it working.
I have analysed the I2C protocol with a logic analyser, and compared it to the NXT. It sents the right values, but for some reason the sensor will not respond (it only receives NACK).
Because there is a bug in the firmware, you have to sent a extra low pulse at SCL (Direct manipulation of I2C pins | Mbed) and I have also changed the SCL frequency to 9,524KHz, because the ultrasonic sensor uses 9,6 KHz as described in the datasheet from LEGO (http://mindstorms.lego.com/en-us/support/files/default.aspx).
Actually it only need that extra puls before the read command, but I can not even write to the sensor. I think that it must have something to do with the frequency, despite I have changed it. Have I done something wrong.

Here is my code:

#include <Wire.h>

int cmd = 0x42 >> 1;//change to 7 bit address
int adr = 0x02 >> 1;//change to 7 bit address
int clockPin = 10;

void setup()
{
  Wire.begin();
  Serial.begin(9600);
}

void loop()
{
  
  //Set I2C frequency to 9,524KHz - the sensor uses 9,6Khz - see p. 223 in atmega328's datasheet
  TWBR = 0xD;//set Bit Rate Register (13)
  TWSR = 0xFB;//set Prescaler Bits (1 and 1 = 64)
  
  
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  
  Wire.beginTransmission(adr);  
  Wire.send(cmd);  
  Wire.endTransmission();
  
  
  delay(0.00005);
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW);
  delay(0.00005);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH); 
  delay(0.00005);
  
  Wire.requestFrom(adr,1);
  while(Wire.available())    // slave may send less than requested
  { 
    char buf = Wire.receive(); // receive a byte as character
    Serial.print(buf);         // print the character
  }
  
  delay(100);
}
/*
' Wires on NXT jack plug.
' Wire colours may vary. Pin 1 is always end nearest latch.
' 1 White +9V
' 2 Black 0V
' 3 Red 0V
' 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.
*/

What am I doing wrong?

Hope to hear from you :slight_smile:

  • Lauszus

I have done some modifications, but it is still not working. I have tried to put two 82K pullup resistors on both the SDA and SCL signal and tried putting 4,7K resistors in serial with the signals, as described in the "LEGO MINDSTORMS NXT Hardware Developer Kit". But it still not working. I have modified the code just a bit, after looking at signal with my logic analyser. The only thing i changed was the delay to match the ones sent by the NXT before reading- first a 128 microseconds high puls, then a 42 microseconds low puls, and then again a 128 microseconds high puls (in the code it says 56 - it just adds on to the delay as used be the standard I2C protocol, in total it is 128 microseconds - apparently the in the code I found, the delay() function, was in seconds and not in milliseconds, as the Arduino).
Here is my modified code:

#include <Wire.h>

int cmd = 0x42 >> 1;//change to 7 bit address
int adr = 0x02 >> 1;//change to 7 bit address
int clockPin = 10;

void setup()
{
  Wire.begin();
  //Set I2C frequency to 9,524KHz - the sensor uses 9,6Khz - see p. 223 in atmega328's datasheet
  TWBR = 0xD;//set Bit Rate Register (13)
  TWSR = 0xFB;//set Prescaler Bits (1 and 1 = 64)
  Serial.begin(9600);
}

void loop()
{
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  
  Wire.beginTransmission(adr);  
  Wire.send(cmd);  
  Wire.endTransmission();
  
  

  delayMicroseconds(56);
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW); 
  delayMicroseconds(42);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH); 
  delayMicroseconds(56);
  
  Wire.requestFrom(adr,1);
  while(Wire.available())    // slave may send less than requested
  { 
    char buf = Wire.receive(); // receive a byte as character
    Serial.print(buf);         // print the character
  }
  
  delay(100);
}
/*
' Wires on NXT jack plug.
' Wire colours may vary. Pin 1 is always end nearest latch.
' 1 White +9V
' 2 Black 0V
' 3 Red 0V
' 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.
*/

Here you can see the output from the NXT:

And here you can see the output from the Arudino:

I have checked the frequency and it seems to be correct. I simply do not know why it will not respond to the request from the Arduino.

I think I found out what is wrong in the "LEGO MINDSTORMS NXT Hardware Developer Kit" it says:

"The digital I/O pins on the NXT cannot be set to open drain directly. Therefore the NXT will drive the digital I/O pins either high or low depending on the situation, i.e., the NXT uses Push-Pull. When the NXT should not control the I/O lines, it will be set as input (e.g., when reading data from a device or when reading acknowledgement)."

But is it possible to use push-pull with the Arduino or send a repeated start?
Thats must be the reason why the sensor is not responding to my commands :slight_smile:

  • Lauszus

I found another I2C library written by Peter Fleury called i2cmaster (http://homepage.hispeed.ch/peterfleury/avr-software.html). I can successfully read from the sensor, but the data is corrupt, because the write command still not works. My code look like this:

#include <i2cmaster.h>

int cmd = 0x08;//Read Product ID
int adr = 0x02;//Ultrasonic Sensor
int clockPin = 10;

int buf[8];

void setup()
{
  Serial.begin(9600);
  i2c_init();//I2C frequency = 9,524KHz
}

void loop()
{    
  pinMode(clockPin, INPUT);//Needed for receiving to work
  digitalWrite(clockPin, HIGH);
  
  i2c_start_wait(adr+I2C_WRITE);
  i2c_write(cmd);
    
  delayMicroseconds(50);//Needed for receiving to work
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW); 
  delayMicroseconds(50);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH); 
  delayMicroseconds(50);
  

  i2c_rep_start(adr+I2C_READ);//Read 5 bytes
  buf[0] = i2c_readAck();
  buf[1] = i2c_readAck();
  buf[2] = i2c_readAck();
  buf[3] = i2c_readAck();
  buf[4] = i2c_readNak();
  i2c_stop();
          
  delay(100);
}

Notice that I do not have to drop the bits, because it takes care of that in the code. If have also modified the library so the frequency is aproximatly 9,6 KHz (See attachment bellow).

The problem with the normal wire.h library was that it did not support repeated start, so I had to use a different one or modify it.

Below you can see the command sent by the Arduino:

It is almost the same as the commands sent by the NXT:

I know it does not send the same command, but I just wanted to see if it could read more bytes.

Bottom of line is that it does not receives ACK when writing or sending the reading command. Does anybody have any ideas how to fix the send command?

i2cmaster.zip (4.34 KB)

I finally got it working. I will post a post soon, describing how to get it working. Stay tuned :slight_smile:

Here is a code which works :slight_smile:

#include <i2cmaster.h>

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(9600);
  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.
*/

For it to work, you have to download the .zip file below. It contains Peter Fleury awesome i2c library (supports repeated start). I have edited the library to work with Arduino, and set the baudrate to 11494,253Hz - the same as the NXT uses. I know that the "LEGO MINDSTORMS NXT Hardware Developer Kit.pdf" says that the sensor uses 9600Hz, but I tuned it to the one the NXT uses, as I thought it would be the best :slight_smile:

i2cmaster.zip (4.92 KB)

Thank you for your sharing. I still struggle to connect to the mindstorms Ultrasound sensor, can I ask you some questions?

  1. Why do you use pin 4 as a clock pin? and you define pin 4 as a input, Is that an mistake or you just on purpose?
  2. Can you describe your circuit ?

Victor

@victorfan

  1. Because for some reason LEGO are using a custom I2C protocol where this extra clock is needed. It is set to input and the high so the internal pullup resister will be enabled. This is needed when the extra clock pulse is not used. See these lines in the source code: NXTShield/NXTShield.cpp at a087ddacbe4dcfdeb0018f0897df3a6f06b6cff3 · TKJElectronics/NXTShield · GitHub.

  2. You can find a library that I have written and the PCB too on Github: GitHub - TKJElectronics/NXTShield: Library for TKJElectronic's NXTShield. You should also check out my blog for more information: TKJ Electronics » Search Results » nxt shield.

Thank you for your information, I will feedback when I get some result.

Hi, Lauszus
I want to do the project like you do, and I use the arduino MEGA 2560 with LEGO Ultrasonic Sensor.
And download the i2cmaster library and use the code you gave.
But it isn't work.(change the pin 4 to pin 20)
I use the 4.7k pullup resistor for SCL and SDA.
Can you help me to find out what wrong is it?