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.
*/
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
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?
#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
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?