rwaldron / johnny-five

JavaScript Robotics and IoT programming framework, developed at Bocoup.
http://johnny-five.io
Other
13.3k stars 1.77k forks source link

IMU class #294

Closed rwaldron closed 9 years ago

rwaldron commented 10 years ago

Starting from conversation here: https://github.com/rwaldron/johnny-five/pull/293#issuecomment-32135307

This is discuss implementing a generic class for I2C IMUs, eg. https://www.sparkfun.com/categories/160

cc @filosganga @soundanalogous @divanvisagie @reconbot

filosganga commented 10 years ago

This is something I have written to emulate I2CDev, it maybe could be useful for other case. I think the q pattern can help here due to nested callback needed to deal with I2C.

function I2C(opts) {

    if (!(this instanceof I2C)) {
    return new I2C(opts);
    }

    var io;

    io = opts.io;

    /** 
     * Write multiple bits in an 8-bit device register.
     * @param address I2C slave device address
     * @param register Register regAddr to write to
     * @param bitStart First bit position to write (0-7)
     * @param length Number of bits to write (not more than 8)
     * @param data Right-aligned value to write
     * @param callback Callback function to call on completition.
     */
    this.writeBits = function(address, register, bitStart, length, data, callback) {

        // From I2CDev:
        //
        //  bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
        //     //      010 value to write
        //     // 76543210 bit numbers
        //     //    xxx   args: bitStart=4, length=3
        //     // 00011100 mask byte
        //     // 10101111 original value (sample)
        //     // 10100011 original & ~mask
        //     // 10101011 masked | value
        //     uint8_t b;
        //     if (readByte(devAddr, regAddr, &b) != 0) {
        //         uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        //         data <<= (bitStart - length + 1); // shift data into correct position
        //         data &= mask; // zero all non-important bits in data
        //         b &= ~(mask); // zero all important bits in existing byte
        //         b |= data; // combine data with existing byte
        //         return writeByte(devAddr, regAddr, b);
        //     } else {
        //         return false;
        //     }
        // }

        var mask;

        io.sendI2CWriteRequest(address, register);
        io.sendI2CReadRequest(address, 1, function(value) {

            var mask;

            if(value != 0) {

                mask = ((1 << length) - 1) << (bitStart - length + 1);
                data = data << (bitStart - length + 1);
                data = data & mask;
                value = value & ~(mask);
                value = value | data;

                io.sendI2CWriteRequest(address, register);
                io.sendI2CWriteRequest(address, value);

                callback(true);
            }
            else {
                callback(false);
            }

        });
    };

    /** 
     * Write a single bit in an 8-bit device register.
     * @param devAddr I2C slave device address
     * @param regAddr Register regAddr to write to
     * @param bitNum Bit position to write (0-7)
     * @param value New bit value to write
     * @return Status of operation (true = success)
     */
    this.writeBit = function(address, register, bit, data, callback) {

        /*
         * From I2CDev
         * bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
         *   uint8_t b;
         *   readByte(devAddr, regAddr, &b);
         *   b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
         *   return writeByte(devAddr, regAddr, b);
         * }
         */

        io.sendI2CWriteRequest(address, register);
        io.sendI2CReadRequest(address, 1, function(value) {

            if(data != 0) {
                value = value | (1 << bit)
            } else {
                value = value & ~(1 << bit)
            }

            io.sendI2CWriteRequest(address, register);
            io.sendI2CWriteRequest(address, value);

            callback();
        }
    };

    /** Read multiple bytes from an 8-bit device register.
     * @param devAddr I2C slave device address
     * @param regAddr First register regAddr to read from
     * @param length Number of bytes to read
     * @param data Buffer to store read data in
     * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
     * @return Number of bytes read (-1 indicates failure)
     */
    this.readBytes = function(address, register, length, data, callback) {

        io.sendI2CWriteRequest(address, register);
        io.sendI2CReadRequest(address, callback);
    };

        /** Read multiple bytes from an 8-bit device register.
     * @param devAddr I2C slave device address
     * @param regAddr First register regAddr to read from
     * @param length Number of bytes to read
     * @param data Buffer to store read data in
     * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
     * @return Number of bytes read (-1 indicates failure)
     */
    this.readByte = function(address, register, data, callback) {
        readByte(address, register, 1, data, callback);
    };

}
soundanalogous commented 10 years ago

You should be able to use a bit mask to implement something like readBit and writeBit if that's what you're after. You'll send and receive the full register byte via Firmata and then do the bit masking in JavaScript. From what I can tell scanning the I2CDev code from SparkFun the readBit and writeBit are just helpers for reading and writing register bits.

The bigger issue you may encounter trying to use an IMU with Firmata is that it will be nearly impossible to get accurate timing and you'll end up with a ton of drift. Maybe the newer IMUs like the ones you are referencing can handle all of the motion processing on chip and send only the results. In that case you shouldn't have an issue. Last I tried using an IMU with Firmata though the processing had to be done in the client and that was a mess.

filosganga commented 10 years ago

Yes, readBit and writeBit are helper in fact, but to abbly the mask, they have to read the register and then apply the mask, it will introduce a lag that I think it is diffucult to deal with.

The mpu9150 have the dmp but is very bad documented, I have some example taken from https://github.com/Pansenti/MPU9150Lib but I don't know how much it is applicable.

soundanalogous commented 10 years ago

You all have a massive task ahead of you. Basically if you want to use this with StandardFirmata under the hood you'll have to port all of these files to JavaScript:

https://github.com/Pansenti/MPU9150Lib/tree/master/libraries/MotionDriver

And all most of these files:

https://github.com/Pansenti/MPU9150Lib/tree/master/libraries/MPU9150Lib

The alternative here is to use ConfigurableFirmata and simply wrap the Arduino library. See this as an example of wrapping a library: https://github.com/firmata/arduino/tree/configurable/examples/DHT11Firmata. This way all you need to do is define the API you want, wrap the Arduino library and expose it by adding a new example to ConfigurableFirmata. Hours vs days here.

The problem you may run into with that route however is that the MPU9150 lib is so huge that you may not have enough flash memory on an Uno. This may only work with a Due or Mega or other 'duino with a lot of memory.

filosganga commented 10 years ago

@soundanalogous can we go in the middle, so using ConfigurableFirmata but instead of the huge MPU9150Lib, we can grab only the raw data from the mpu9150, so that the drifting should be alleviated, is it doable in your opinion?

soundanalogous commented 10 years ago

Sounds like a reasonable approach especially if anyone wants to use other sensors and actuators in the same application in addition to the MPU9150.

soundanalogous commented 10 years ago

If you need additional featuers (analog in, pwm, digital i/o, etc) in addition to I2C then you can use the ConfigurableFirmata.ino sketch as a base but write the MPU9150 wrapper as an example (like DHT11Firmata). Just don't include features you don't need (such as OneWire and Ethernet support for example) or you'll run out of flash memory.

filosganga commented 10 years ago

This one for sparkFun:

https://github.com/sparkfun/MPU-9150_Breakout/blob/master/firmware/MPU6050/

looks like a good starting point, with a diet on all undocumented dmp functions it should be reasonable small. It uses the I2CDev but i think, because many other use the I2CDev as well, it is reasonable to keep there...maybe.

filosganga commented 10 years ago

Uhm I didn't manage to update the arduino 1.5.5 with the new Firmata.

I have cloned the repo, then run the release.sh.

I have copied the extracted zip (the Arduino-1.5.x-Firmata-2.4.0.zip) into

/Applications/Arduino.app/Contents/Resources/Java/libraries/Firmata

But the utility staff was not on the zip, so I have added them as well.

I have also copied into

/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata.

and now I have problem compiling the ConfigurableFirmata example:

/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:21: error: expected constructor, destructor, or type conversion before '*' token
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'void reportAnalogInputCallback(byte, int)':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:25: error: 'AnalogInputFirmataInstance' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: At global scope:
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:28: error: 'AnalogInputFirmata' has not been declared
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:28: error: ISO C++ forbids declaration of 'AnalogInputFirmata' with no type
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'int AnalogInputFirmata()':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:30: error: 'AnalogInputFirmataInstance' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:30: error: invalid use of 'this' in non-member function
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:31: error: 'analogInputsToReport' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: At global scope:
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:40: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'void reportAnalog(byte, int)':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:44: error: 'analogInputsToReport' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:46: error: 'analogInputsToReport' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: At global scope:
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:53: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:70: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:78: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'boolean handleSysex(byte, byte, byte*)':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:89: error: 'handleAnalogFirmataSysex' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: At global scope:
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:93: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'void reset()':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:96: error: 'analogInputsToReport' was not declared in this scope
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: At global scope:
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:99: error: 'AnalogInputFirmata' is not a class or namespace
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp: In function 'void report()':
/Applications/Arduino.app/Contents/Resources/Java/hardware/arduino/avr/libraries/Firmata/src/utility/AnalogInputFirmata.cpp:106: error: 'analogInputsToReport' was not declared in this scope
soundanalogous commented 10 years ago

I'm having the same issue in Arduino 1.5.5. However it will compile in Arduino 1.0.5. The arduino team keeps changing the way the libraries are compiled in Arduino 1.5.x so I suspect there's an issue recursively compiling the utility directory. I'll look into it. In the mean time, download Arduino 1.0.5.

Also, I'll push an update to the configurable branch to add the utility directory.

filosganga commented 10 years ago

Have added a first MPU9150 integration, now I would like to know how to exdcute some initialization stuff like calling the MPU6050::initialize() function. I think it should be a custom SYSEX command. Any tips how to add a command? Or any example I can look at?

Thanks.

filosganga commented 10 years ago

Regarding the other sparkfun IMU, most of them are I2c, but instead of integrated like the mpu9150, they are simple breackout of 2/3 different component (The MPU6050 doesn't have the magnetometer but it can be added externally).

The Razor IMU instead integrate an Atmel on the board and comunicate trought serial. So on aduino UNO we cannot use it because the only serial is used by USB and the Firmata doesn't support SoftwareSerial, is it right?

soundanalogous commented 10 years ago

I posted a proposal for SW serial a while ago but have received zero feedback on it (as with most things Firmata). There are limitations (and conflicts) with SW serial and firmata though because of the interrupts that SW serial uses.

Regarding MPU9150 integration. If you take the ConfigurableFirmata approach you don't need to use I2C (from the Firmata client - J5, etc). You'd create a custom sysex API for it. See this as an example proposal for a new feature. Also look at StepperFirmata for an example implementation of a custom Sysex api.

filosganga commented 10 years ago

I have a first version of this integration ready, but I have a few questions:

var firmata = require("firmata");

firmata.Board.prototype.....

But it doesn't work. Any other way? Or have we to hack the Firmata module?

The code to add to firmata:

var IMU_COMMAND = 0x65,
      IMU_REPLY = 0x66,
      IMU_INITIALIZE = 0x01,
      IMU_DESTROY = 0x02,
      IMU_READ_MOTION = 0x20;

SYSEX_RESPONSE[IMU_REPLY] = function (board) {

    var device = (board.currentBuffer[2] & 0x7F) | ((board.currentBuffer[3] & 0x7F) << 7);
    var command = (board.currentBuffer[4] & 0x7F) | ((board.currentBuffer[5] & 0x7F) << 7);

    console.log("IMU: command: " + command.toString(16) + ", device: " + device + ".");

    if (command == IMU_INITIALIZE) {
        var address = (board.currentBuffer[6] & 0x7F) | ((board.currentBuffer[7] & 0x7F) << 7);
        board.emit('imu-initialized-' + device, address);
    } else if(command == IMU_DESTROY) {
        board.emit('imu-destroy-' + device);
    } else if(command == IMU_READ_MOTION) {

         var data = {
            ax: (board.currentBuffer[6] & 0x7F) | ((board.currentBuffer[7] & 0x7F) << 7),
            ay: (board.currentBuffer[8] & 0x7F) | ((board.currentBuffer[9] & 0x7F) << 7),
            az: (board.currentBuffer[10] & 0x7F) | ((board.currentBuffer[11] & 0x7F) << 7),
            gx: (board.currentBuffer[12] & 0x7F) | ((board.currentBuffer[13] & 0x7F) << 7),
            gy: (board.currentBuffer[14] & 0x7F) | ((board.currentBuffer[15] & 0x7F) << 7),
            gz: (board.currentBuffer[16] & 0x7F) | ((board.currentBuffer[17] & 0x7F) << 7),
            mx: (board.currentBuffer[18] & 0x7F) | ((board.currentBuffer[19] & 0x7F) << 7),
            my: (board.currentBuffer[20] & 0x7F) | ((board.currentBuffer[21] & 0x7F) << 7),
            mz: (board.currentBuffer[22] & 0x7F) | ((board.currentBuffer[23] & 0x7F) << 7)
         };

        board.emit('imu-read-motion-' + device, data);
    }
};

Board.prototype.sendImuInitialize = function(device, address, callback) {

    var data = [
        START_SYSEX,
        IMU_COMMAND,
        device,
        IMU_INITIALIZE,
        address,
        END_SYSEX
    ];

    this.sp.write(new Buffer(data));
    this.once('imu-initialized-' + device, callback);
};

Board.prototype.sendImuDestroy = function(device, callback) {

    var data = [
        START_SYSEX,
        IMU_COMMAND,
        device,
        IMU_DESTROY,
        END_SYSEX
    ];

    this.sp.write(new Buffer(data));
    this.once('imu-destroy-' + device, callback);
};

Board.prototype.sendImuReadMotion = function (device, callback) {

    var data = [
        START_SYSEX,
        IMU_COMMAND,
        device,
        IMU_READ_MOTION,
        END_SYSEX
    ];

    this.sp.write(new Buffer(data));
    this.once('imu-read-motion-' + device, callback);
};

Let me know what do you think about that...I am very proud it is working actually :D:D

filosganga commented 10 years ago

I have hacked Firmata from the boards.js file. Don't know if this is the right approach, maybe we need some type of callback to permit access to backend Firmata in some way.

filosganga commented 10 years ago

The last output from my code is:

Magnetization: mx: 35, my: 255, mz: 148
Acceleration ax: 44, ay: 254, az: 4
Rotation gx: 1, gy: 136, gz: 66
Magnetization: mx: 43, my: 255, mz: 154
Acceleration ax: 16, ay: 254, az: 136
Rotation gx: 0, gy: 20, gz: 66
Magnetization: mx: 36, my: 255, mz: 117
Acceleration ax: 216, ay: 253, az: 176
Rotation gx: 0, gy: 100, gz: 65
Magnetization: mx: 37, my: 255, mz: 169
Acceleration ax: 76, ay: 254, az: 196
Rotation gx: 0, gy: 212, gz: 65
Magnetization: mx: 14, my: 255, mz: 147
Acceleration ax: 80, ay: 254, az: 176
Rotation gx: 0, gy: 28, gz: 66
Magnetization: mx: 30, my: 255, mz: 151
Acceleration ax: 252, ay: 253, az: 28
Rotation gx: 1, gy: 32, gz: 66
Magnetization: mx: 255, my: 254, mz: 157
Acceleration ax: 88, ay: 254, az: 248
Rotation gx: 0, gy: 200, gz: 65
Magnetization: mx: 24, my: 255, mz: 160

There is some problem....my is always 255.

filosganga commented 10 years ago

Hey @soundanalogous, I have managed to solve most of the problem, but one. Passing signed int from arduino to Javascript. Shifting 8 bits works in arduino, and works oin Javascript, but they behave in different way, so I cannot reassemble the bytes sent from arduino in Javascript.

-32000 >> 8 in Arduino is 16259 -32000 >> 8 in js is -125

Any idea how to solve that?

soundanalogous commented 10 years ago

see this solution: https://github.com/rwaldron/johnny-five/blob/master/lib/compass.js#L461

filosganga commented 10 years ago

Coop thanks this was the missing piece. I would suggest to group this byte/bit related functions in one module. It will be very useful. On Jan 14, 2014 7:02 PM, "Jeff Hoefs" notifications@github.com wrote:

see this solution: https://github.com/rwaldron/johnny-five/blob/master/lib/compass.js#L461

— Reply to this email directly or view it on GitHubhttps://github.com/rwaldron/johnny-five/issues/294#issuecomment-32295538 .

filosganga commented 10 years ago

The sparkfun driver has some bugs regarding the magnetometer reading. Basically it is low endian by handled has big endian there are also some perf issues. So I am going to use this one:

https://github.com/Pansenti/MPU9150Lib/blob/master/libraries/MotionDriver/inv_mpu.cpp

It offers low level access to all the features.

rwaldron commented 10 years ago

@filosganga any updates? :)

dtex commented 9 years ago

An IMU class was added by @BrianGenisio in #544 . This issue also discusses adding software serial to Firmata among other things, but they seem tangential to the original request. If we close this do any of the other topics need to be migrated to new issues (possibly on other repos)?

soundanalogous commented 9 years ago

I don't think the SoftwareSerial discussion here is in depth enough to require migration. If there is not already a central thread on SoftwareSerial needs for J5, the would be helpful to add or consolidate. It's something I still intend to work on for Firmata since it keeps coming up... made good progress a year ago then got distracted and forgot about it.

dtex commented 9 years ago

The only other mention of softwareSerial is in #486 but again, it's just tangential (I can't do X until softwareSerial).

If anyone is here because they are looking for information on softwareSerial in Firmata (and by extension Johnny-Five), https://github.com/firmata/arduino/issues/97 is where you should go for more info (or to contribute).

I'm closing this.