Lego NXT – Arduino Truck


New & improved NXT truck and LeJOS running brick !

The beast after the lifting...

So more details about the changes:

  1. The NXT brick is running LeJOS now, nicely programmed from NetBeans 6.8 and remotely downloaded through bluetooth (did I mention how much nicer and easier it is to program now !? :) )
  2. New direction and gears. A servo is now used for the direction as it offers more control. Also rather than controlling both this servo and the one for the gearing through the Arduino, I control them directly from the NXT, unsing NXTServo from mindsensors.com(I have the previous version)

    Servos for new direction and gears

  3. Uses my new dedicated IR board(that understands the protocol from the Syma Helicopter remote control). However, it’s a shame but I still need an Arduino between this board and the Lego brick, as the NXT can only talk I2C and this board can only talk its own serial protocol.

    Dedicated IR board (attiny45 based) and the corresponding Arduino that simply translated from its own bespoke serial protocol to I2C

  4. The NXTservo controlling the 2 servos

    NXT servo (version 1) and its battery

Now here’s the code being run on the Arduino:

1) the custom serial part that talks to the dedicated IR board

const byte BYTE_SIZE = 8;
const int T = 500;        // in microseconds
const int DELTA = T / 2;
const int T0_MIN = T - DELTA;
const int T0_MAX = T + DELTA;
const int T1_MIN = 3 * T0_MIN;
const int T1_MAX = 3 * T0_MAX;
const int START_MIN = 8 * T0_MIN;
const int START_MAX = 8 * T0_MAX;
const byte IR_PIN = 2;
const byte _signalSize = BYTE_SIZE * sizeof(_sharedData);  // sizeof is directly in bytes
long _startPulseTicks = -1;
int _pulseCount = -1;
long _pulses[_signalSize];
long _currPulse;
bool _interruptContinue = true;
void customSerialSetup(){
    // on PIN 2 !!
    attachInterrupt(0, onInterrupt, CHANGE);
}

byte getBit(long pulseSize){
    if ((pulseSize > T0_MIN) && (pulseSize < T0_MAX)) return 0;     if ((pulseSize > T1_MIN) && (pulseSize < T1_MAX)) return 1;
    return 2;
}

byte _currentCode = 0;
bool updateCodes(){
    for(int k = 0; k < sizeof(_sharedData); k++){
        _currentCode = 0; // init

        for (byte i = 0; i < BYTE_SIZE; i++ ){              byte bit = getBit(_pulses[k * BYTE_SIZE + i]);              if (bit > 1) return false; //if any single bit in the whole signal is bad
             if (bit == 1) _currentCode += 1 << i;         }         _sharedData[k] = _currentCode;     }     return true; } void checkSignalDone() {     // FINAL, we've read what we wanted transform into codes and let the caller know !     if (_pulseCount == _signalSize){         // do this only if the main code is NOT in the middle of sending stuff... otherwise just loose them...         if(_sharedStatus != 1) {             _sharedStatus = 2;             if(updateCodes()) _sharedStatus = 0; //the codes are ready to be sent ONLY if everything went OK         }         // end of this command, so reInit so that we wait for another start         _pulseCount = -1;     } } void onInterrupt() {     if(digitalRead(IR_PIN) == HIGH){         // interested in HIGH pulses         _startPulseTicks = micros();     }else if(_startPulseTicks > 0){
        // if low AND a pulse is started
        _currPulse = micros() - _startPulseTicks;
        _startPulseTicks = -1; // will wait for the next start
        if (_currPulse > START_MIN && _currPulse < START_MAX){              //start counting pulses              _pulseCount = 0;          }else if (_pulseCount >= 0) {
            //continue counting pulses
            _pulses[_pulseCount] = _currPulse;
            _pulseCount++;

            checkSignalDone();
          }
     }
}

2) and the main part of the code

/*
* This program waits for a custom serial transmission on interrupt 0 (pin 2)
* and then sends whatever bytes it has received on I2C to the master.
*/

#include
byte _sharedStatus = 2; // 0  the codes are ready to be sent - 1 the main code is sending - 2 waiting
byte _sharedData[5];
bool _dataHasChanged = false;
bool _i2csending = false;
byte _dataToSend[6]; // plus 1 control byte

void setup() {
    Wire.begin(0x12);
    Wire.onRequest(sendData);
    customSerialSetup();
}

void loop(){
    if(_sharedStatus == 0 && !_i2csending){ // his test + assertion are NOT atomic... that's life I don't know what I can do...
        _sharedStatus = 1; // sending codes, the customSerial interrupts should not modify the _data
        for(int i=0; i> 1;

        _dataToSend[5] = 6;
        _sharedStatus = 2;
        _dataHasChanged = true;
     }
}

void sendData(){
   if(_dataHasChanged){
        _i2csending = true;
        Wire.send(_dataToSend, 6);
        _dataHasChanged = false;
        _i2csending = false;
    }
}

And finally the JAVA code on the NXT brick itself !

CherryPicker.java


import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.MSC;

public class CherryPicker {
private final MSC _servoController = new MSC(SensorPort.S1);
private final Direction _directionManager = new Direction(_servoController.servo1);
private final MyMotor _gasManager = new MyMotor(MotorPort.A);
private final IRSymaSensor _IRCmdManager = new IRSymaSensor(SensorPort.S2, 0x12);
private final Crane _craneManager = new Crane(_servoController.servo2, MotorPort.B);

private boolean _craneMode = true;
private boolean _mode = false;

public CherryPicker() {
_IRCmdManager.addListener(new IRSymaSensor.IRSymaSensorListener() {
public void commandReceived(int speed, int dir, int vertical, int trimmer, boolean leftButton, boolean rightButton) {
if(leftButton) _mode = !_mode;
if(rightButton) _craneMode = ! _craneMode;
if(_mode){
int newSpeed = speed - 20;
if(newSpeed < 0) newSpeed = 0; int way = (trimmer > 30 ? 1 : -1);
_gasManager.setSpeed(newSpeed*way);

if(_craneMode){
_directionManager.setDirection(-dir);
_craneManager.moveVertically(vertical * 4);
}else{
_craneManager.moveHorizontally(dir * 2);
}
}else{
_gasManager.stop();
_craneManager.stop();
}
}
});

_IRCmdManager.addListener(getLCDStatusListener());
}

private IRSymaSensor.IRSymaSensorListener getLCDStatusListener() {
return new IRSymaSensor.IRSymaSensorListener() {
public void commandReceived(int speed, int dir, int vertical, int trimmer, boolean leftButton, boolean rightButton) {
LCD.clear();
LCD.drawString("M:" + _mode, 0, 0);
LCD.drawString("CM:" + _craneMode, 9, 0);
LCD.drawString("Lb:" + leftButton, 0, 1);
LCD.drawString("Rb:" + rightButton, 9, 1);
LCD.drawString("S" + speed, 0, 2);
LCD.drawString("D" + dir, 5, 2);
LCD.drawString("V" + vertical, 10, 2);
LCD.drawString("T" + trimmer, 0, 3);
LCD.refresh();
}
};
}

/**
* Entry point in the whole project.
*/

public static void main(String[] args) throws Exception {

// ALWAYS listen for ESCAPE !
Button.ESCAPE.addButtonListener(
new ButtonListener() {
public void buttonPressed(Button b) {
System.exit(0);
}

public void buttonReleased(Button b) {
throw new UnsupportedOperationException("Not supported yet.");
}
});

new CherryPicker();
}
}

Crane.java

import lejos.nxt.MotorPort;
import lejos.nxt.addon.MServo;
public class Crane {
private final ModeSelector _modeSelector;
private final MyMotor _motor;

public Crane(MServo servo, MotorPort motorPort) {
_modeSelector = new ModeSelector(servo);
_motor = new MyMotor(motorPort);
}

public void moveVertically(int speed){
_motor.setSpeed(speed);
_modeSelector.modeVertical();
}

public void moveHorizontally(int speed){
_motor.setSpeed(speed);
_modeSelector.modeHorizontal();
}

public void stop() {
_motor.stop();
_modeSelector.modeNone();
}

private static class ModeSelector {

private static final int MODE_NONE_ANGLE = 90;
private static final int MODE_VERTICAL_ANGLE = 160;
private static final int MODE_HORIZONTAL_ANGLE = 20;

private final MServo _servo;

public ModeSelector(MServo servo){
_servo = servo;
}

public void modeNone(){
goToAngle(MODE_NONE_ANGLE);
}

public void modeVertical(){
goToAngle(MODE_VERTICAL_ANGLE);
}

public void modeHorizontal() {
goToAngle(MODE_HORIZONTAL_ANGLE);
}

private void goToAngle(int angle){
int sign = 1;
if(_servo.getAngle() > angle) sign = -1;
_servo.setAngle(angle + 20*sign);
while(Math.signum(angle - _servo.getAngle()) == sign) try { Thread.sleep(20); } catch (InterruptedException ex) {}
_servo.setAngle(angle);
}
}
}

Direction.java

import lejos.nxt.addon.MServo;
public class Direction {
private final static int REAL_CENTER = 80;
private final static int REAL_MAX_LEFT = 40;
private final static int REAL_MAX_RIGHT = 120;
private final static int INPUT_MAX_LEFT = REAL_MAX_LEFT - REAL_CENTER;
private final static int INPUT_MAX_RIGHT = REAL_MAX_RIGHT - REAL_CENTER;
private final MServo _servo;

public Direction(MServo servo){
_servo = servo;
}

/**
* In degrees.
* Converts -90 .. 90 angles into ANGLE_MAX_LEFT .. ANGLE_MAX_RIGHT
* @param angle 0 is straight. 0 is right
* @return the real angle
*/

public void setDirection(int angle){
int newAngle = angle;
if(newAngle < INPUT_MAX_LEFT) newAngle = INPUT_MAX_LEFT; else if(newAngle > INPUT_MAX_RIGHT) newAngle = INPUT_MAX_RIGHT;
_servo.setAngle(REAL_CENTER + newAngle);
}

/**
* In degrees
* Converts ANGLE_MAX_LEFT .. ANGLE_MAX_RIGHT angles to -90 .. 90
*/

public int getDirection() {
return _servo.getAngle() - REAL_CENTER;
}
}

MyMotor.java

import lejos.nxt.Motor;
import lejos.nxt.TachoMotorPort;

public class MyMotor extends Motor {
private final static int REAL_MIN_SPEED = 0;
private final static int REAL_MAX_SPEED = 900;
private final static int INPUT_MIN_SPEED = 0;
private final static int INPUT_MAX_SPEED = 100;
public MyMotor(TachoMotorPort port){
super(port);
smoothAcceleration(true);
//regulateSpeed(true);
}

@Override
public int getSpeed(){
int absSpeed = convert(super.getSpeed(), REAL_MIN_SPEED, REAL_MAX_SPEED, INPUT_MIN_SPEED, INPUT_MAX_SPEED);
if(isForward()) return absSpeed;
else return -absSpeed;
}

@Override
public void setSpeed(int speed){
if(speed == 0) stop();
else {
super.setSpeed(convert(Math.abs(speed), INPUT_MIN_SPEED, INPUT_MAX_SPEED, REAL_MIN_SPEED, REAL_MAX_SPEED));
if(speed > 0) forward();
else backward();
}
}

private static int convert(float input, int inMin, int inMax, int outMin, int outMax) {
return Math.round(((input - inMin) / (inMax - inMin)) * (outMax - outMin) + outMin);
}
}

IRSymaSensor.java

import java.util.ArrayList;
import java.util.List;
import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import lejos.nxt.LCD;
import lejos.nxt.SensorConstants;
public class IRSymaSensor {
private static final byte CMD_SIZE = 6; //last char is for checking
private static final byte LEFT_RIGHT_MIDDLE = 64;
private static final byte VERTICAL_MIDDLE = 16;

private boolean _stopThread = false;
private final I2CSensor _sensor;
private final List _listeners = new ArrayList();
private final byte[]    _data = new byte[CMD_SIZE];

public IRSymaSensor(I2CPort port, int address){
_sensor = new I2CSensor(port, I2CPort.LEGO_MODE);
port.setType(SensorConstants.TYPE_HISPEED);
_sensor.setAddress(address);

// start a new thread monitoring the sensor and calling the listeners when something arrives
new Thread(){
@Override
public void run(){
while(! _stopThread) {
_data[CMD_SIZE - 1] = 0; // re-init the contol byte
if(_sensor.getData(0x00, _data, CMD_SIZE) == 0 && _data[CMD_SIZE - 1] == CMD_SIZE) {
for(IRSymaSensorListener listener : _listeners) {
listener.commandReceived(_data[0], _data[1] - LEFT_RIGHT_MIDDLE, _data[2] - VERTICAL_MIDDLE, _data[3], _data[4] == 1, _data[4] == 2);
}
}
}
}
}.start();
}

@Override
protected void finalize() {
_stopThread = true;
}

public void addListener(IRSymaSensorListener listener) {
_listeners.add(listener);
}

public static interface IRSymaSensorListener {
public void commandReceived(int speed, int dir, int vertical, int trimmer, boolean leftButton, boolean rightButton);
}
}

__________________________________________________________________________________

This is the initial post, with the truck from 2 years ago (Dec2008)

Quickly (1week however :)) putting together : -

  1. a Lego Technic truck
  2. Nxt brick from (Lego Mindstorms)
  3. an Arduino (freeduino) board
  4. a universal remote control (configured to use the Sony protocol)
  5. an IR receiver from a toy
LegoNxtArduinoROBOT

LegoNxtArduinoROBOT Note the Servo used to switch the arm's gearbox (up/down VS left/right)

LegoNxtArduinoRobot2

Here's the freeduino + the salvaged IR receiver.

2 main problems when doing this project: – get the remote to talk to the Arduino (use Sony remote control protocol) – get the Arduino to talk to the Nxt over I2C (really the only “issue” that did take some time…) The big black/grey “head” is from an Erector toy, and it contains : – IR Receiver (Red – VCC / Yellow – GND / Green – OUT). The out is connected through a 333Ohms resistor) – Green LED (Orange – / Brown +) I2C connections

      NXT:                          Arduino:          Description:
      -------------------           ------------      ------------
      2 GND     (black)     <--->   Gnd               Ground
      5 DIGIAI0 (yellow)    <--->   Analog In 5       SCL
      6 DIGIAI1 (blue)      <--->   Analog In 4       SDA

      No pullup resistors required (these are handled by the Wire-Libray
      at the Arduino using the builtin resistors in the Atmega chip)

NXT Connector

(copyright Wikipedia)

Sensors are connected to the NXT brick using a 6-position modular connector that features both analog and digital interfaces. The analog interface is backward-compatible (using an adapter) with the older Robotics Invention System. The digital interface is capable of both I2C and RS-485 communication.

NXT Sensor Interface Pinout
Pin Name Function Color Pin Numbering
1 ANA Analog interface, +9V Supply Pin 1 - ANA
white
2 GND Ground Pin 2 - GND
black
3 GND Ground Pin 3 - GND
red
4 IPOWERA +4.3V Supply Pin 4 - IPOWERA
green
5 DIGIAI0 I2C Clock (SCL), RS-485 A Pin 5 - DIGIAI0
yellow
6 DIGIAI1 I2C Data (SDA), RS-485 B Pin 6 - DIGIAI1
blue

I’d love to hear your comments.

dan

P.S. very useful links, thanks to their respective authors for the (indirect) help…

1) interesting but somewhat simpler as only 2 Arduino involved

2) infrared communication thread

3) really the most useful, explaining the tricks of communicating with the NXT

Here’s the code used…

Arduino

#include
#include

#define DEBUG 1                 //Serial connection must be started to debug

#define ARDUINO_ADDR 0x43
#define IR_PIN 7                //Sensor pin 1 wired through a 220 ohm resistor
#define LED_PIN 6                //"Ready to Recieve" flag, not needed but nice
#define START_MIN 2200            //Start bit threshold (Microseconds)
#define BIN_1 1000                //Binary 1 threshold (Microseconds)
#define BIN_0 400                 //Binary 0 threshold (Microseconds)
#define SIZE 12
#define PULSE_TIMEOUT 10000                // in microseconds
#define NO_KEY 254
#define PIN_LIGHT 12

#define SERVO_PIN 9
#define SERVO_MIN 650
#define SERVO_MAX 2350

volatile int m_lastKey = NO_KEY;
volatile boolean m_light = false;
Servo myServo;

void setup() {
pinMode(LED_PIN, OUTPUT);        //This shows when we're ready to recieve
pinMode(IR_PIN, INPUT);
pinMode(PIN_LIGHT, OUTPUT);
//attachInterrupt(0, setOpts, LOW);    // interrupt 0, run setOpts(), interrupt when ir_pin goes LOW

digitalWrite(LED_PIN, LOW);        //not ready yet

Serial.begin(115200);

Wire.begin(ARDUINO_ADDR);  // join i2c bus as slave 4
Wire.onRequest(i2cRequestHandler);
Wire.onReceive(i2cReceiveHandler);

myServo.attach(SERVO_PIN, SERVO_MIN, SERVO_MAX);
myServo.write(0);
}

void loop() {
int key = getIRKey();            //Fetch the key

if(DEBUG){
Serial.print("Key: ");
Serial.println(key);
}

if(key &gt;= 0){
m_lastKey = key;
}
}

void doCmdAction(byte cmd){
if(cmd == 12){
// light
if(m_light){
digitalWrite(PIN_LIGHT, LOW);
m_light = false;
}else{
digitalWrite(PIN_LIGHT, HIGH);
m_light = true;
}
}else if(cmd == 15){
// turn servo left
myServo.write(50);
}else if(cmd == 16){
// turn servo right
myServo.write(130);
}else if(cmd == 17){
// turn servo middle
myServo.write(90);
}
}

void i2cRequestHandler(){
Wire.send(getByte(m_lastKey));              // sends one byte
m_lastKey = NO_KEY;
}

void i2cReceiveHandler(int numBytes){
while(Wire.available() &gt; 0) {
doCmdAction(Wire.receive());
}
}

/* void setOpts(){
}  */

byte getByte(int key){
if(key == 752) return 1;
if(key == 2800) return 2;
if(key == 720) return 3;
if(key == 3280) return 4;
if(key == 2672) return 5;
if(key == 144) return 6;
if(key == 2192) return 7;
if(key == 3536) return 8;

// 1..5 on the remote command DIGI PINS 8-12
if(key == 16) return 12;
if(key == 1168) return 15;
if(key == 3216) return 16;
if(key == 656) return 17;

return 249;
}

int getIRKey() {
int data[SIZE];

int time = 0;
while( pulseIn(IR_PIN, LOW, PULSE_TIMEOUT)  10){
return -2;
}

time++;
}

digitalWrite(LED_PIN, HIGH);       //Ok, i'm ready to recieve

for(int i=0; i BIN_0) {        //is it a 0?
data[i] = 0;
} else {
data[i] = 2;              //Flag the data as invalid; I don't know what it is!
}
}
}

for(int i=0;i 1) {
return -1;                 //Return -1 on invalid data
}
}

int result = 0;
int seed = 1;
for(int i=SIZE-1; i&gt;=0; i--) {          //Convert bits to integer
if(data[i] == 1) {
result += seed;
}
seed = seed * 2;
}
return result;                 //Return key number
}

NXT Brick – NXC language used

mainIR.nxc

#include "motors.nxc"
#include "i2cArduino.nxc"

#define SPEED_INCR 10
#define ANGLE_INCR 60

task run(){
byte irCMD = getIRCmd();
while(true){
irCMD = getIRCmd();
NumOut(10, LCD_LINE1, irCMD, true);

if(irCMD > 0 && irCMD  10){
sendIRCmd(irCMD);
}
}
//Wait(100);
}
}

task main(){
initSens();
Precedes(run);
}

i2cArduino.nxc

#define S_ARDUINO IN_1
#define ARDUINO_ADDR 0x43
#define ARDUINO_ADDR_SEND (ARDUINO_ADDR << 1)
#define ARDUINO_ADDR_RECV (ARDUINO_ADDR_SEND + 1)

#define CMD_FWD 1
#define CMD_BKW 2
#define CMD_LEF 3
#define CMD_RGT 4
#define CMD_STP 5
#define CMD_AUX1 6
#define CMD_AUX2 7
#define CMD_AUX_STOP 8

byte m_inBuff[] = {ARDUINO_ADDR_SEND};

byte getIRCmd(){
byte l_outBuff[];
byte l_count = 1;

while (I2CCheckStatus(S_ARDUINO) == STAT_COMM_PENDING); // wait for previous command to finish
Wait(100);  //important !

if (! I2CBytes(S_ARDUINO, m_inBuff, l_count, l_outBuff)) {
return 253;
}

return l_outBuff[0];
}

byte sendIRCmd(byte val){
byte cmdBuf[2];
cmdBuf[0] = ARDUINO_ADDR_SEND;
cmdBuf[1] = val;

while (I2CCheckStatus(S_ARDUINO) == STAT_COMM_PENDING); // wait for previous command to finish
Wait(100);  //important !

byte result = I2CWrite(S_ARDUINO, 1, cmdBuf);
Wait(100);  //important !
if (result  NO_ERR) {
TextOut(0, LCD_LINE2, "SEND CMD Error: ");
NumOut(30, LCD_LINE3, result);
Wait(5000);
}

return result;
}

motors.nxc

#include "sensors.nxc"

#define MOTOR_PWR OUT_A
#define MOTOR_STEER OUT_B
#define MOTOR_ARM OUT_C

#define DIR_MAX_ANGLE 120
#define DIR_SPEED 100
#define DIR_ANGLE_ATONCE 5
#define MV_MAX_SPEED 100
#define ARM_SPEED 60

int m_dir_currAngle = 0;
mutex m_dir_mutex;
int m_mv_currSpeed = 0;
mutex m_mv_mutex;

/* LOW LEVEL */
void setWheels(int angle){
Acquire(m_dir_mutex);

if (angle  DIR_MAX_ANGLE)
{
angle = DIR_MAX_ANGLE;
}

int deltaAngle = angle - m_dir_currAngle;
RotateMotor(MOTOR_STEER, DIR_SPEED, deltaAngle);
m_dir_currAngle = angle;

Release(m_dir_mutex);
}

void setSpeed(int speed){
if(speed  MV_MAX_SPEED)
{
speed =MV_MAX_SPEED;
}

Acquire(m_mv_mutex);
m_mv_currSpeed = speed;
OnFwd (MOTOR_PWR, m_mv_currSpeed);
Release(m_mv_mutex);
}

/* END LOW LEVEL */

void turn(int angle){
int unit = angle / abs(angle); // 1 or -1
int wheelsAngle = DIR_MAX_ANGLE * unit;
int speed = 40 * unit;

for(int i=0; i &lt; abs(angle); i+= 2*DIR_ANGLE_ATONCE){
setWheels(wheelsAngle);
setSpeed(speed);

int currCompass = abs(M_compass);
while(abs(abs(M_compass) - currCompass) < DIR_ANGLE_ATONCE) Wait(100);

//go backwards now
setSpeed(0);
setWheels(-wheelsAngle);
setSpeed(-speed);

currCompass = abs(M_compass);
while(abs(abs(M_compass) - currCompass) < DIR_ANGLE_ATONCE) Wait(100);
setSpeed(0);
}

setSpeed(0);
setWheels(0);
}

/* 0..10*/
void armHeight(int height){
int ARM_LOW = -20;
int ARM_HI = -80;

if(height  10) height = 10;
int target = height * (ARM_HI - ARM_LOW)/10  + ARM_LOW;

if(M_accY &lt; target){
OnRev(MOTOR_ARM, ARM_SPEED);
while(M_accY  target){
Wait(100);
}
}
Off(MOTOR_ARM);

// wait to stabilize, then check everything is OK
Wait(500);
if(M_accY &lt; target){
OnRev(MOTOR_ARM, ARM_SPEED/2);
while(M_accY  target){
Wait(100);
}
}
Off(MOTOR_ARM);
}

sensors.nxc


#include "../common/_I2Csensors.nxc"

#define CIRCLE_UNITS 360

int M_us;
int M_compass;
int M_accX;
int M_accY;
int M_accZ;

void updateAllSensorValues(){
M_us = (getUS() + getUS() + getUS()) / 3 ;
M_compass = (getCompass() + getCompass() + getCompass()) / 3 ;
M_accX = (getAccX() + getAccX() + getAccX()) / 3 ;
M_accY = (getAccY() + getAccY() + getAccY()) / 3 ;
M_accZ = (getAccZ() + getAccZ() + getAccZ()) / 3 ;
Wait(100);
}

void initSens(){
//BTCheck(BT_CONN);
initSensors();
COMP_setAdvancedMode(true);
//COMP_setUSAMode(true);

updateAllSensorValues();
}

task sensors(){
while(true){
updateAllSensorValues();
}
}

int addAngle(int initAngle, int delta){
int result = initAngle + delta;
if(result &gt; CIRCLE_UNITS){
result = result - CIRCLE_UNITS;
}else if(result &lt; 0){
result = result + CIRCLE_UNITS;
}

return result;
}

_i2cSensors.nxc

#define S_ACCEL IN_4
#define S_COMP IN_4
#define S_US IN_2
#define S_IR IN_1
#define S_SERVO IN_3

// Devices Addresses
#define ADDR_ACCEL 0xA2
#define ADDR_COMP 0xB2
#define ADDR_IR 0x02
#define ADDR_SERVO 0xB0

// Registers
#define REG_DATA_ACCEL_X 0x45
#define REG_DATA_ACCEL_Y 0x47
#define REG_DATA_ACCEL_Z 0x49
#define REG_DATA_COMP 0x42
#define REG_DATA_IR 0x42
#define REG_QUICK_SERVO 0x5A

#define REG_CMD_COMP 0x41
#define REG_CMD_ACCEL 0x41

byte bufLSWrite_ACCX[] = {ADDR_ACCEL, REG_DATA_ACCEL_X};
byte bufLSWrite_ACCY[] = {ADDR_ACCEL, REG_DATA_ACCEL_Y};
byte bufLSWrite_ACCZ[] = {ADDR_ACCEL, REG_DATA_ACCEL_Z};
byte bufLSWrite_COMP[] = {ADDR_COMP, REG_DATA_COMP};
byte bufLSWrite_IR[] = {ADDR_IR, REG_DATA_IR};

#define BT_CONN 0
#define OUTBOX 1

int getIntFrom2Bytes(byte p_LSB, byte p_MSB){
int l_result =  p_MSB & 0xFF;
l_result &lt; NO_ERR)
status = I2CCheckStatus(port);
Stop(status &lt; NO_ERR);

return status;
} */

mutex m_I2C_mutex;
int sendCmd(byte port, byte addr, byte register, byte cmd) {
byte cmdBuf[3];
cmdBuf[0] = addr;
cmdBuf[1] = register;
cmdBuf[2] = cmd;

Acquire(m_I2C_mutex);
while (I2CCheckStatus(port) == STAT_COMM_PENDING); // wait for previous command to finish
byte result = I2CWrite(port, 0, cmdBuf);
Wait(100);  //important !
if (result  NO_ERR) {
TextOut(0, LCD_LINE2, "SEND CMD Error: ");
NumOut(30, LCD_LINE3, result);
Wait(5000);
}
Release(m_I2C_mutex);

return result;
}

void COMP_calibrateCompass(){
// BEGIN calibration mode
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x43);
// wait 2 minutes
Wait (600000);
// END calibration mode
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x44);
}

// Advanced 0-3600 or Basic Lego Sonar mode 0-255
int COMP_setAdvancedMode(bool mode){
if (mode){
return sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x49);
}else{
return sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x42);
}
}

// USA/Europe sampling frequency (60/50 Hz)
int COMP_setUSAMode(bool mode){
if (mode){
return sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x55);
}else{
return sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0x45);
}
}

/*0xA0, 0xAA, 0xA5, */
void changeCOMPAddress(byte newAddress){
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0xA0);
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0xAA);
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, 0xA5);
sendCmd(S_COMP, ADDR_COMP, REG_CMD_COMP, newAddress);
}

/*0xA0, 0xAA, 0xA5, */
void changeACCELAddress(byte newAddress){
sendCmd(S_ACCEL, ADDR_ACCEL, REG_CMD_ACCEL, 0xA0);
sendCmd(S_ACCEL, ADDR_ACCEL, REG_CMD_ACCEL, 0xAA);
sendCmd(S_ACCEL, ADDR_ACCEL, REG_CMD_ACCEL, 0xA5);
sendCmd(S_ACCEL, ADDR_ACCEL, REG_CMD_ACCEL, newAddress);

}
/**** END ADVANCED ****/

// quick registers 5A - 61, resolution 50-255 (min 150)
int setServoQuick(short servoNo, int pos){
if(pos  240) pos = 240;

return sendCmd(S_SERVO, ADDR_SERVO, REG_QUICK_SERVO + servoNo, pos);
}

sub BTCheck(int conn){
int stat = BluetoothStatus(conn);
if (stat != NO_ERR){
string msg = NumToStr(stat);
TextOut(0, LCD_LINE2, "BTCheck Err: " + msg);
Wait(5000);
Stop(true);
}
}
About these ads

19 Responses to Lego NXT – Arduino Truck

  1. Pingback: Arduino – 3chanel IR remote control from mini helicopter – processing « Robotics / Electronics / Physical Computing

  2. Pingback: ATtiny Hacks: Infrared guidance and navigation « FOOTBALL, SEX & ALCOHOL

  3. Pingback: ATtiny Hacks: Infrared guidance and navigation - Hack a Day

  4. Henry says:

    i’m so sorry, for bother you oance more :)

    as I had mention, the basic I2C is working, but not sure how, it would be possible to send a string to the arduino, for example, send a string, with a registry to perform other task, like do a quick calculation based on a given text (converted form string to a float)

    I have the function to convert the string to float and perfirm the calculation, however if i want to send shuch register plus the string, this confuses me a bit

    I will appreciate any help and advice

    Thanks

    • trandi says:

      Hi Henry,

      I’m not sure I fully understand what yor requirement is here, but in any case it seems to be more involved.
      If you can’t find your answer in the examples on my blog, maybe it would be worth posting a message on one of the Arduino forums…

      dan

  5. Henry says:

    Many Thanks, I got it working, few silly errors, but I got a basic I2C communication woking

    Thanks again,

  6. Henry says:

    Thanks

    one last thing, regrading the pull-up resistor;O have two 82K resitor, that are connected from
    Arduino pin 4 — 82K — Pin 6 NXT
    and Arduino pin 5 — 82K — pin 5 NXT

    Thanks

    • trandi says:

      As their names say “pull-UP” these resistors should be connected between the SCL and SDA wires and VCC !
      I’m not sure from your question if you’re doing this, or if you’re connecting the Arduino and the NXT THROUGH the resistors, which would be wrong.
      Again these 2 wires go directly between the 2 I2C devices AND between each wire and +5V you have these 2 resistors.

      dan

  7. Henry says:

    wow, you just hve impress me, it was so quick

    Thanks, Although I’m trying to do a small test, but so far I have unable to make it work, and its driving me mad,

    • trandi says:

      I know and understand how frustrating it can be… I too spend sometimes entire days or week-ends on some silly blocking issues…

      What are you using on the NXT brick : Java or NXC ?
      Also do you have an I2C sensor with which you could test your code on the NXT before trying to connect to the Arduino ?

      In any case, look around my blog, there are plenty of other examples of I2C communication both for Arduino and the NXT.
      Here for example you have Java code for LeJOS and Arduino code that I use to make the 2 communicate.
      Look for the “SlaveBoard.java” and “TigerTankBB_Slave.pde”.

      Hope this helps,
      dan

    • Henry says:

      Hi wonder, if you can help me please

      I got this but unable to meake it work, and I wudl like to ask for your advice

      Arduino Code ——-

      #include
      // Address for the Arduino to announce itself as
      #define ID 0x43
      #define ADDRESS ID
      #define ADDRESS_SEND (ID << 1)
      #define ADDRESS_RECV (ADDRESS_SEND – 1)

      // Original information
      uint8_t SensorVersion[8] = "V0.1 ";
      uint8_t SensorName[8] = "Sensor ";
      uint8_t SensorType[8] = "Arduino";

      byte x;
      int ledPin5 = 5;
      int ledPin6 = 6;

      void setup() {
      pinMode(ledPin5,OUTPUT);
      pinMode(ledPin6,OUTPUT);
      digitalWrite(ledPin5, LOW);
      digitalWrite(ledPin6, LOW);
      blinkLED ();
      Serial.begin(9600);
      Wire.begin(ID);
      Serial.println (ADDRESS);
      Wire.onReceive(receiveEvent);
      Wire.onRequest(requestEvent);

      }

      void blinkLED()
      {
      digitalWrite(ledPin5, HIGH);
      digitalWrite(ledPin6, HIGH);
      delay(500);
      digitalWrite(ledPin5, LOW);
      digitalWrite(ledPin6, LOW);
      }

      void loop(){
      delay (50);
      //Serial.println(x);
      }
      //———————————I2C Events——————————-//

      void receiveEvent(int howMany)
      {
      x = Wire.receive(); // receive byte as an integer
      Serial.println(x);
      }

      void requestEvent(){
      if (x == 0x00){
      Wire.send(SensorVersion, 8);
      digitalWrite(ledPin5, HIGH);
      delay(100);
      digitalWrite(ledPin5, LOW);

      }
      else if (x == 0x08){
      Wire.send(SensorName, 8);
      digitalWrite(ledPin6, HIGH);
      delay(100);
      digitalWrite(ledPin6, LOW);

      }
      else if (x == 0x10){
      Wire.send(SensorType, 8);

      }
      }

      —————————————————–END—————————
      NCX Code

      ——————————-

      // ARDUINO NXT interface
      #define ARDUINO_ADDRESS 0x43
      #define ADDRESS_SEND (ARDUINO_ADDRESS << 1)
      #define ADDRESS_RECV (ADDRESS_SEND – 1)
      #define ARDUINO_PORT IN_1

      string temp;
      byte outbuf[];
      byte cmdbuf[];

      string i2cReadString(byte port, byte adr, byte reg, byte cnt)
      {
      ArrayBuild(cmdbuf,adr, reg);

      byte nByteReady = 0;
      while (I2CCheckStatus(port) == STAT_COMM_PENDING) ;
      Wait (100);
      // I2CWrite(port, 8, cmdbuf);
      if(I2CBytes(port, cmdbuf, cnt, outbuf))
      {
      temp = ByteArrayToStr(outbuf);
      }
      return temp;
      }

      void ShowSensorInfo(byte port, byte address)
      {
      ClearScreen();
      TextOut(0, LCD_LINE2, i2cReadString(port, address, 0x00, 8), false);
      TextOut(0, LCD_LINE3, i2cReadString(port, address, 0x08, 8), false);
      TextOut(0, LCD_LINE4, i2cReadString(port, address, 0x10, 8), false);

      }

      task main() {
      byte nByteReady = 0;
      byte cmdbuf[];
      SetSensorLowspeed(ARDUINO_PORT);
      Wait(100);
      while(true) {
      ShowSensorInfo(ARDUINO_PORT,ADDRESS_SEND);
      }
      }

      any help would be welcome

      Thanks

      • trandi says:

        I won’t pretent I’ve read in detail all the code you’ve pasted here, but here are a few comments:

        Arduino:

        – you don’t need to have 2 different addresses for read / send, the library takes care of this. Just use the ID or 0x43 (it’s a pecularity of NXC that you need to manually give the 2 addresses) – this seems actually to be what you’re doing so just delete the other constants

        – in the “receiveEvent()” it’s always a good idea to use something like “if(Wire.available()) Wire.receive();” to make sure you read everything from the buffer, otherwise strange things may happen …

        NXC:
        – if you look at my example it’s “#define ARDUINO_ADDR_RECV (ARDUINO_ADDR_SEND + 1)” and NOT ” – 1″

        – when you do “ShowSensorInfo(ARDUINO_PORT,ADDRESS_SEND);” you’re obviously trying to READ data from the I2C bus, but you’re giving the SEND address… this really doesn’t make sense…

        Overall, I’d say use the EXACT code I’ve posted in my original post and try to make it work, before trying to change it.

        hope this helps,
        dan

  8. Henry says:

    Hi

    I don’t fully understand the (ARDUINO_ADDR &lt;&lt; 1), could you please clarify how is this used

    Thanks

    • Henry says:

      I ment (ARDUINO_ADDR & amp;lt;& amp;lt; 1)

    • trandi says:

      I’ve just fixed it, it was a copy / paste issue.
      It should be “(ARDUINO_ADDR << 1)” .
      It all has to do with how the I2C protocol works, the fact that addresses are on 7 rather than 8 bits and that the send / receive actions are signalled by using actually 2 different CONSECUTIVE addresses.

      Hope this helps,
      dan

  9. Kristian Sloth Lauszus says:

    Hallo. Very nice project. Can you tell me if the I2C is a 3.3V, 4.2V or a 5V signal?

    • trandi says:

      Hi, thank you for your comment.

      As you suggest I2C “per se” is independent of the voltage, it’s just a protocol.
      In this concrete case of Lego NXT brick, I think it’s 4.3V. I’m saying this because on the connector there’s a green wire that has this voltage (there’s also the white one at 9V, but that one is for analog motors I think).
      But I’m NOT sure, they could put that 4.3V Vcc there and have the signal lines at 3.3 or 5V…

      HOWEVER, I’ve simply connected tha SCL and SDA DIRECTLY to the Arduino pins that works at 5V, and it seems to be working happily…

      hope this helps,
      Dan

  10. visitor says:

    Interesting. I’ve been looking for so long for a way of interfacing my Arduino with some Lego NXT spare parts…
    Thanks a lot for sharing your work.

  11. visitor says:

    wow… very impressing !

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Follow

Get every new post delivered to your Inbox.

Join 132 other followers

%d bloggers like this: