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 Note the Servo used to switch the arm's gearbox (up/down VS left/right)

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 I²C and RS-485 communication.

NXT Sensor Interface Pinout

Pin	Name	Function	Color	Pin Numbering
1	ANA	Analog interface, +9V Supply	white
2	GND	Ground	black
3	GND	Ground	red
4	IPOWERA	+4.3V Supply	green
5	DIGIAI0	I2C Clock (SCL), RS-485 A	yellow
6	DIGIAI1	I2C Data (SDA), RS-485 B	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 >= 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() > 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>=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 < 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 > CIRCLE_UNITS){
result = result - CIRCLE_UNITS;
}else if(result < 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 < NO_ERR)
status = I2CCheckStatus(port);
Stop(status < 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);
}
}