How to read a mindsensor nxt compass, I don't understand the example.... Options

[antosecret]

As said in the topic title, I bought a mindsensor compass sensor.... I read the documentation and tried the example they give for Nxt.
The 2 nbc programs give me an error when I try to compile them, and the nxc one does not show any value about orientation read from the sensor.

Can anyone explain me how to read precise values using nxc??

[John Hansen]

If you download the latest test release of NBC and BricxCC then you will have an early version of beta 35 which includes some additional API functions written specifically for the MindSensors devices. There is a new SensorMSCompass(port) API function which should return the compass heading value that you are trying to read. Just set the sensor as a lowspeed or I2C sensor just like you do for the Ultrasonic sensor. Also make sure you keep the sensor 5 inches away from the motors or the NXT itself so that the magnetic fields generated by the motors or the NXT do not significantly affect the compass heading. If you are using NBC then you can use the ReadSensorMSCompass(port, result) API function.

CODE

task main() {
  SetSensorLowspeed(S4);
  while (true) {
    NumOut(0, LCD_LINE5, SensorMSCompass(S4), true);
    Wait(100);
  }
}

Using the documentation for the Mindsensors compass you could also construct a low level call to the device using ReadI2CBytes in NBC or I2CBytes in NXC.

CODE

#define cmpAddr 0x02;

byte bufCmd[]     = {cmpAddr, 0x41, 0x49};
byte bufLSWrite[] = {cmpAddr, 0x42};

task main() {
  SetSensorLowspeed(S1);
//  I2CWrite(S1, 0, bufCmd);
  string outbuf;
  byte count;
  while (true) {
    byte bLo, bHi;
    count = 2;
    int value;
    if (I2CBytes(S1, bufLSWrite, count, outbuf)) {
      bLo = outbuf[0];
      bHi = outbuf[1];
      value = bHi*256+bLo;
    }
    else {
      value = 0;
    }
    NumOut(10, LCD_LINE5, value, true);
    Wait(250);
  }
}

[antosecret]

If you download the latest test release of NBC and BricxCC then you will have an early version of beta 35 which includes some additional API functions written specifically for the MindSensors devices.

I did it.

There is a new SensorMSCompass(port) API function which should return the compass heading value that you are trying to read. Just set the sensor as a lowspeed or I2C sensor just like you do for the Ultrasonic sensor. Also make sure you keep the sensor 5 inches away from the motors or the NXT itself so that the magnetic fields generated by the motors or the NXT do not significantly affect the compass heading.

CODE

task main() {
  SetSensorLowspeed(S4);
  while (true) {
    NumOut(0, LCD_LINE5, SensorMSCompass(S4), true);
    Wait(100);
  }
}

It seems to work properly but it gives me results from 0 to 244. What if I want to have result out of 360 degrees??? I read something about it on the guide by Mindsensor but it is not so clear for me (if you know about any tutorial for beginners that I can read about advanced sensors, it could be the solution of all my problems).

Using the documentation for the Mindsensors compass you could also construct a low level call to the device using ReadI2CBytes in NBC or I2CBytes in NXC.

CODE

#define cmpAddr 0x02;

byte bufCmd[]     = {cmpAddr, 0x41, 0x49};
byte bufLSWrite[] = {cmpAddr, 0x42};

task main() {
  SetSensorLowspeed(S1);
//  I2CWrite(S1, 0, bufCmd);
  string outbuf;
  byte count;
  while (true) {
    byte bLo, bHi;
    count = 2;
    int value;
    if (I2CBytes(S1, bufLSWrite, count, outbuf)) {
      bLo = outbuf[0];
      bHi = outbuf[1];
      value = bHi*256+bLo;
    }
    else {
      value = 0;
    }
    NumOut(10, LCD_LINE5, value, true);
    Wait(250);
  }
}

This piece of code is not working. I can read on the screen only 0 for most of the time, and occasionally very big numbers.
What's wrong with it?

Thank you for your answer

[John Hansen]

Make sure your NXT has version 1.05 of the standard NXT firmware installed. It is available on the MINSTORMS support/update website.

http://mindstorms.lego.com/support/updates/

The docs say that 0..360 is mapped to 0..255 for LEGO Sonar compatible mode. In the advanced mode you should get a value from 0..36000. To switch the compass into advanced mode you need to write 0x02, 0x41, 0x49. That is the commented out line in the second program. The two programs I posted use different ports. Did you make sure you switched sensor ports when you ran the second program? You can include the declaration of the byte array from the second program and the I2CWrite call from the second program in the first program if you want.

CODE

#define cmpAddr 0x02;
byte bufCmd[]     = {cmpAddr, 0x41, 0x49};

task main() {
  SetSensorLowspeed(S4);
  I2CWrite(S4, 0, bufCmd);
  Wait(100);
  while (true) {
    NumOut(0, LCD_LINE5, SensorMSCompass(S4), true);
    Wait(100);
  }
}

[antosecret]

Make sure your NXT has version 1.05 of the standard NXT firmware installed. It is available on the MINSTORMS support/update website.

I have FW NBC/NXC 1.06a build 1401081218 (as said on my nxt screen). Is it ok? Should i downgrade it to the 1.05 standard one?

Did you make sure you switched sensor ports when you ran the second program?

Yes, I did. What else could I try to have the 2nd program working properly?

You can include the declaration of the byte array from the second program and the I2CWrite call from the second program in the first program if you want.

CODE

#define cmpAddr 0x02;
byte bufCmd[]     = {cmpAddr, 0x41, 0x49};

task main() {
  SetSensorLowspeed(S4);
  I2CWrite(S4, 0, bufCmd);
  Wait(100);
  while (true) {
    NumOut(0, LCD_LINE5, SensorMSCompass(S4), true);
    Wait(100);
  }
}

I downloaded the program on my nxt but it is still showing me values up to 250.

[John Hansen]

Unfortunately, I do not have the MindSensors compass 2.0 to test this API with. I am basing the API code on the MindSensors documentation. I have a really old (1.0) version of the sensor.

Does anyone else have the MindSensors compass that they can test these API functions with? It would be great to get another set of data points regarding whether they work correctly or not.

I will see if I can get a new version of the device to test with.

John Hansen

[antosecret]

I will see if I can get a new version of the device to test with.

Ok. Thank you for your help...
I'll let you know if I solve the problem alone...

Should I write to the mindsensor support center ?!

Did you try to compile the two nbc examples from mindsensor?
When I try to do that I get lots of errors.... Most of them are about double declarations...

Here there are the links of the 2 files:
http://www.mindsensors.com/index.php?modul...;JAS_File_id=51
http://www.mindsensors.com/index.php?modul...;JAS_File_id=50


Antonio

[John Hansen]

Here is an edited version of the 2.0 sample code that you linked to above. The other code is for the previous version of the compass hardware which you probably do not have. Both programs were written for a really old version of NBC which is why you get the compiler errors. I have simplified the code a bit and modernized it to compile with the latest NBC beta release.

CODE

dseg    segment
;------- declarations -------
Heading  dword
lsBytesRead byte
thePort byte IN_4

lo_byte byte
hi_byte byte
RLSBRetVal byte

bZeroRead byte
bBadRead byte
bResultLT0 byte
bLSStatLT0 byte
bLSStatEQ0 byte

lswArgs TCommLSWrite
lsrArgs TCommLSRead
lscsArgs TCommLSCheckStatus

CMPS_cmd_I byte[] 0x02, 0x41, 0x49
CMPS_Rd_lo byte[] 0x02, 0x42
CMPS_Rd_hi byte[] 0x02, 0x43
readBuf    byte[]

dseg    ends

; -------------- program code --------------
thread main
; configure the sensor
  SetSensorLowspeed(thePort)

; initialize values
    mov    lswArgs.Port, thePort
    mov    lsrArgs.Port, thePort
    set    lswArgs.ReturnLen, 1
    set    lsrArgs.BufferLen, 1
StartAgain:

; write convert Integer command  I 0x49
    mov    lswArgs.Buffer, CMPS_cmd_I
    call ReadLSByte
  wait 50

; read first byte
    mov    lswArgs.Buffer, CMPS_Rd_lo
    call ReadLSByte
    mov lo_byte, RLSBRetVal

; read second byte
    mov    lswArgs.Buffer, CMPS_Rd_hi
    call ReadLSByte
  mov hi_byte, RLSBRetVal

; use the two bytes to calculate heading value
    mul Heading, hi_byte, 256
    add Heading, Heading, lo_byte

  NumOutEx(10, 8, Heading, TRUE)

; begin all over again
    jmp StartAgain
    exit
endt

subroutine CheckLSStat
    mov    lscsArgs.Port, thePort
    syscall    CommLSCheckStatus, lscsArgs
    tst    LT, bLSStatLT0, lscsArgs.Result
    tst    EQ, bLSStatEQ0, lscsArgs.Result
    return
ends

subroutine ReadLSByte
    syscall    CommLSWrite, lswArgs
    wait 10

DoCheckStatus:
    call CheckLSStat
; check return values of subroutine and repeat if    needed
    or    bBadRead, bLSStatEQ0, bLSStatLT0
    brtst    EQ, DoCheckStatus, bBadRead
; ls status checks out okay
    syscall    CommLSRead, lsrArgs
    mov    readBuf, lsrArgs.Buffer

    arrsize    lsBytesRead, readBuf

    brtst EQ, ElseIf1, lsBytesRead
  index RLSBRetVal, readBuf, NA

    jmp EndIf1
ElseIf1:
    set RLSBRetVal, 0
EndIf1:
    return
ends

[Lren]

John's NXC code as written does not work. Here is the simplest revision that does work

CODE

#define cmpAddr 0x02
byte bufCmd[]     = {cmpAddr, 0x41, 0x49};

task main() {
  SetSensorLowspeed(S4);
  Wait(100);
  I2CWrite(S4, 0, bufCmd);
  Wait(100);
  while (true) {
    NumOut(0, LCD_LINE5, SensorMSCompass(S4), true);
    Wait(100);
  }
}

Yes the wait is necessary but the really important change is removing the semi-colon from the end of the 0x02.
I would actually recommend replacing the first two Wait(100) with this:

CODE

Wait(100);
while (LowspeedCheckStatus(S4) == STAT_COMM_PENDING);
deviceStatus = LowspeedCheckStatus(S4);
      if (deviceStatus < 0)
       {
       NumOut(0, LCD_LINE1, deviceStatus, true);
       PlayFile("! Attention.rso");
       Wait(1000);
       Stop(deviceStatus < 0);
       }

Sometimes the sensor dos not start correctly. Probably it was incorrectly shut down the last time.

[antosecret]

John's NXC code as written does not work. Here is the simplest revision that does work

You are right. I tested your code with my nxt and it seems to work properly.

Sometimes the sensor does not start correctly. Probably it was incorrectly shut down the last time.

I want to use my compass sensor in a local competition. What if it doesn't start properly???
Is there a way to restart it without turning off the brick???

Thank you for your help.

Antosecret

[antosecret]

By the way, I have anothee question: Is it possible to reset a sensor (such as the ultrasonic one to the factory default)???
I am asking it because today I'm having some problem with mine....

Since today it needs a wait(100) before to be read... otherwise it stops the program....
and in a second part of the code (another simultaneus task) after 4 or 5 readings it stops that task...

And sometimes it says file error! without starting the program at all

It was all right yesterday.... I can only think I could have written something wrong on its register tryng to have my compass sensor working....

What can I do?

My firmware now is nbc/nxc 1.06a
Here there is a part of my program...
The unworking tasks are VaiFinoUs (it does not work if i remove the wait(100) before the while) and the Schermo (it controls the screen, after 2 or 3 loops it crash down just before printing the us sensor value)

CODE

/*
IN_1 = COMPASS SENSOR
IN_2 = ULTRASONIC SENSOR
OUT_A = THIRD MOTOR
OUT_B = RIGHT MOTOR
OUT_C = LEFT MOTOR

*/


#define P 40
#define I 40
#define D 90

#define cmpAddr 0x02 // costanti per il compass sensor
byte bufCmd[] = {cmpAddr, 0x41, 0x49};


#define DistanzaUltrasuoni 20 //distanza di rilevamento oggetti del sensore ultrasonico
#define ScreenUpdate 250 // frequenza aggiornamento dei valori sullo schermo
int rot = 1;

int veldx, velsx, velmin;
int orientament;
int CompIniz;
int x = 0,y = 0, dist;

unsigned long Tempo0;
int funz;
void VaiAvanti(int distanza, int potenza);
void VaiFinoUS(int valore1);
void Gira(int gradi);
sub evita();


task Schermo(){
ClearScreen();
TextOut(2, LCD_LINE1, "Velsx: ");
TextOut(2, LCD_LINE2, "Veldx: ");
TextOut(2, LCD_LINE4, "Routine: ");
TextOut(2, LCD_LINE3, "US Sensor: ");
TextOut(2, LCD_LINE5, "Velmin: ");
TextOut(2, LCD_LINE6, "Coord: ");
TextOut(2,LCD_LINE8, "Compass: ");

while(true){
TextOut(50, LCD_LINE1, " ");
TextOut(50, LCD_LINE2, " ");
TextOut(50, LCD_LINE3, " ");
TextOut(50, LCD_LINE4, " ");
TextOut(50, LCD_LINE5, " ");
TextOut(50, LCD_LINE6, " ");
TextOut(20, LCD_LINE7, " ");
TextOut(50, LCD_LINE8, " ");
NumOut(50, LCD_LINE1, velsx);
NumOut(50, LCD_LINE2, veldx);
NumOut(50, LCD_LINE3, SensorUS(IN_2));
NumOut(50, LCD_LINE5, velmin);
NumOut(50, LCD_LINE6, x);
NumOut(80, LCD_LINE6, y);
NumOut(20, LCD_LINE7, orientament);
NumOut(50, LCD_LINE7, funz);
// NumOut(50, LCD_LINE8, SensorMSCompass(S1)- CompIniz);

}
Wait(ScreenUpdate);
PlayTone(700,30);
}
}


void VaiAvanti(distanza, potenza){ // funz = 1
// it let the robot move forward of a given distance
}

void Gira(gradi){ // funz = 3
PlayToneEx(162, 400, 2, false);
funz = 3;
int verso = 100;
if (gradi < 0) verso = -100;
if (gradi > 0) verso = 100;
RotateMotorEx(OUT_BC, PotSterz, abs(gradi), verso, true, true);
//RotateMotorPID(OUT_C, PotSterz, gradi, P, I, D);
MotorRotationCount(OUT_B);
funz = 0;
}


sub evita(){
PlayFile("RilevatoOstacol.rso");
Gira(rot*Giro90);
VaiAvanti(DistOst, PotAvant);
Gira(-rot*Giro90);
rot = rot==1 ? -1 : 1;
}



void VaiFinoUS(valore1){ //funz = 2 // 2 = ultrasuoni, secondo sensore opzionale
// mentre cammina controllo attivo della velocità
funz = 2;
Off(OUT_BC);
Wait(100);
while((SensorUS(IN_2) > valore1)){
OnRev(OUT_BC,75);
//aggCoordinate();
CtrlVel();
Wait(100);

}
Off(OUT_BC);
}


sub aggCoordinate(or, k) {}

task main(){ //funz = 0
SetSensorLowspeed(S1); // inizializzazione compass sensor
Wait(100);
I2CWrite(S1, 0, bufCmd);
Wait(100);
CompIniz = SensorMSCompass(S1);

velmin=0; // inizializzazione programma
funz = 0;
orientament = 1;
SetSensorLowspeed(IN_2);
ResetRotationCount(OUT_A);
start Tempo;
start velocit;
start Schermo;
start velminima;

while(true){
VaiFinoUS(DistanzaUltrasuoni);
evita();
}

Off(OUT_BC);
}

[antosecret]

I fixed it by reading the sensor value and putting it in a global variable which is read by both the tasks...
But I still do not undurstand why two days ago it was working properly and yesterday it wasn't...
Any idea?

By the way thank you all for your help

[Lren]

I cannot tell you why it used to work. However the reason using the global variable fixed it is because you have written a multitasking program without providing any locking. Using a global variable (mostly) removed the need for locking.

Some of the NXC routines have their own locking to prevent separate tasks accessing resources at the same time. However I suspect it is still really easy for two tasks to contend for resources. Such resource problems generally cause unpredictable behavior i.e. the program runs perfecly well then at random times it goes competely haywire. At work I find these are some of the worse problems to solve because they are so random.

If you really need to use multi tasking I suggest you put mutex around anything likely to cause problems. If you don't need multi tasking don't use it.

P.S. I also suggest you log to a file not to the screen. It's far more likely to catch the robot doing something you didn't expect.

[antosecret]

I cannot tell you why it used to work. However the reason using the global variable fixed it is because you have written a multitasking program without providing any locking. Using a global variable (mostly) removed the need for locking.

If you really need to use multi tasking I suggest you put mutex around anything likely to cause problems. If you don't need multi tasking don't use it.

I'll try to use mutex... It seems to be a very good idea!

P.S. I also suggest you log to a file not to the screen. It's far more likely to catch the robot doing something you didn't expect.

I need to log to the screen so that i can know exactly what's going wrong and when....

Thank you for your help!!!!

Antonio

[antosecret]

Ok. I'm able to read the sensor. But I still have some problems!!!

For example... It suddenly jumps from 70 to about 250 degrees... And I don't know how to fix it.
I'm tryng to use it to have my robot turning of "exactly" 90 degrees.

Here there are the codes I'm using (written on Lren's example):

Code for calibrating:

CODE

#define cmpAddr 0x02
byte bufCmd[] = {cmpAddr, 0x41, 0x49, 0x45, 0x4C};

task main(){ //funz = 0
SetSensorLowspeed(S1); // starting compass sensor
Wait(100);
I2CWrite(S1, 0, bufCmd);
Wait(100);
I2CWrite(S4, 0, 0x43);
until (ButtonPressed(BTNCENTER,true)); // I turn it for two or three rotations and then I press the button to end the calibration
I2CWrite(S4, 0, 0x44);

}

Code for reading:

CODE

#define cmpAddr 0x02
byte bufCmd[] = {cmpAddr, 0x41, 0x49, 0x45, 0x4C};
int CompIniz;

task main() {
SetSensorLowspeed(S1); // starting compass sensor
Wait(100);
I2CWrite(S1, 0, bufCmd);
Wait(100);
while (!ButtonPressed(BTNCENTER,true)) { //read the values from the sensor
NumOut(0, LCD_LINE5, SensorMSCompass(S1), true);
Wait(100);
}
}

What does european or american sampling frequency mean???

[Lren]

I don't think you can stack up commands like that in I2C. You have to do them one at at time i.e. 0x02 0x41 must come before each command byte.

There is no need to calibrate the sensor unless it will always be in an unusual magnetic field. On a moving robot it is a waste of time.

The code I originally posted works fine. I suggest you try this with a Wait(1000) because the Wait(100) is too fast to read especially if you are hand holding the compass. Remember values will be 0-3599 and can easily jump +/- 20 i.e. two degrees.

[Brian Davis]

There is no need to calibrate the sensor unless it will always be in an unusual magnetic field. On a moving robot it is a waste of time.

Hmm. I'm not sure this is exactly true.. after all, the Compass sensor is in an unusual magnetic field - around motors, batteries, etc. There's a reason the calibrate ability is build into things like the Compass, and it's not so you can use it around MRI machines (now there's a scary thought).

--
Brian Davis

[Lren]

I'll explain further. I have a Mindsensors compass but I am certainly not an expert in it's use.

I have found that it must be kept away from the NXT. This seems to be mostly because of the speaker. Twenty cm seems to be plenty of separation. I think it should also kept away from the motors. If you had to locate the compass close to the NXT calibration might be some use. To be most accurate you would have to make sure the motors were active when you did the calibration.

However I want to use my compass inside my wooden house. I have tested this environment. It is full of magnetic anomalies. I have some steel beams and iron reinforcing which makes a considerable difference. The refrigerator will completely reverse the compass. A one off calibration is not going to work here. I need a magnetic map.

[Brian Davis]

Twenty cm seems to be plenty of separation.

At least using the Hitechnic sensor, I get reliable performance with a much lower separation that that. YMMV.

I want to use my compass inside my wooden house. I have tested this environment. It is full of magnetic anomalies.

Again, YMMV. I've had no problems in my house as it rolls around, but I do remember Steve Hassenplug demonstrating Omni at NI Week. While it worked great down on the main floor, up in the room where we gave our talk it always homed in on a single spot on the floor, and then "orbited" that spot. Talk about an unfortunate magnetic anomaly .

--
Brian Davis