UART IMU sensor for EV3

Some time ago I made a Segway-like robot using Lego NXT set. I based it on this mathematical model. The controller used the control loop with 8 ms period, so it required the gyroscope sensor that could read samples faster than 8 ms. I had two gyroscope sensors: Hitechnic Gyro Sensor and Mindsensors AbsoluteIMU.
Hitechnic sensor is an analog one and it can produce samples very fast, but it has significant drift and that depends on the battery voltage (see this post). AbsoluteIMU is a 3-axis sensor that produce more accurate measurements, but it is an I2C sensor that works quite slow on NXT and EV3 platforms. Reading sample time is about 25 ms.
Fortunately, there is a way to speed up I2C bus on NXT platform. LeJOS for NXT can turn I2C sensor to high-speed mode where reading sample time is about 2 ms.
But when I ported my code to EV3 platform, I found that high speed I2C support work incorrectly on EV3. So I could use the sensor in low-speed mode only. It was about a year ago.
I wanted an IMU that combines gyroscope and accelerometer and would be able to read samples faster that 8 ms.
EV3 supports new UART sensors that can support very high communication speed (up to 460 kbit/sec). I have found an article that can be a good starting point to create own UART sensors for EV3.
But I wanted to have a complete device that can be mounted to Lego robot, so I needed to fit it into the an existing Lego sensor case. I used Lego Light Sensor as a case for my IMU sensor.
During working on the sensors I tested three IMU chips: LSM330DLC, LSM9DS0 and LSM6DS3.
The first one is a cheap 6DOF sensor, the second one is a 9DOF sensor (I want to tests 9DOF sensor fusion algorithm on it), and the last one is a low-noise 6DOF sensor.

Sensors use UART speed 115200 bps to be compatible with EV3 Soft-UART ports 3 and 4. Ports 1 and 2 allow to use speed up to 460800 bps. But due to a bug in EV3 firmware it actually uses frequency 485294 bps, so it is necessary to use this value to be able run UART in hi-speed mode (in sensor descriptor we should specify speed 460800, but set uart speed to 485294).  I have prepared a class uart_speed that calculates correct UART speed, compatible with EV3.

Mathematical Model of Lego EV3 Motor

Some time ago I calculated Lego NXT motor parameters to use them in mathematical modeling of Lego motor. Also I used them to calculate controller constants for Segway-like robot.

When I bought Lego EV3 set, I wanted to port my Segway program to EV3. But I need to build EV3 motor model to calculate balance controller constants. In this post I described how I measured and estimated the motor's parameters.

Laser Sensor for Lego NXT

This sensor was made for experiments with triangular localization.

 For triangulation  we need to have three beacons and a sensor that can detect beacons. The laser sensor seems as the best solution for this experiment.

Motor controller with feed-forward for Lego NXT

Goals
During experiments with Java version of Lego motor controller and built-in LeJOS motor controller I found that they use different approaches to implement smooth acceleration. Lego controller uses integer arithmetics and implements non-linear acceleration algorithm. It divides the acceleration distance to the speed delta and gets the interval to increase speed for 1 degree/sec. Each time when the motor covers the interval, the controller increases its speed for 1 degree/sec. This algorithms is simple but acceleration is non-linear. LeJOS uses different approach. The version I tested had problems with smooth acceleration. But LeJOS team has published an updated version of motor controller that works well. This version implements linear acceleration approach using floating point arithmetic. Also LeJOS controller uses much smalled discretization interval (interval between regulation cycles). Lego controller uses 100ms interval, LeJOS controller uses 4ms interval. This small regulation interval has some advantages and disadvantages: it provides more fast reaction to change of external load but requires more CPU time, especially being implemented in Java. Lego controller is implemented in native ARM code and works faster.

I wanted to make a controller that has best qualities of LeJOS controller but requires less CPU time. I tried to increase discretization interval but found that this controller has problems with rotation to a specified angle. It could not reach the target position with zero speed and requires additional time to reach the target point.

Compare motor controllers of LeJOS and Lego firmware

In this post I'm going to compare two motor controllers: from Lego Mindstorms firmware and from LeJOS class library. I use LeJOS in my robot programming, because it much more powerful than other programming languages I used (NXT-G, NXC). But I found a strange behavior of the motor controller in my last project and wanted to study how it works and compare it to PID-controller from default Lego firmware.