tag:blogger.com,1999:blog-7298122087962749992024-03-20T18:11:37.236+03:00NXTLego Mindstorms programmingMaximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.comBlogger5125tag:blogger.com,1999:blog-729812208796274999.post-30049918001000118842016-07-07T15:50:00.001+03:002019-04-09T15:51:27.595+03:00UART IMU sensor for EV3<div dir="ltr" style="text-align: left;" trbidi="on">
Some time ago I made a Segway-like robot using Lego NXT set. I based it on <a href="http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs--self-balancing-two-wheeled-robot--controller-design">this</a> 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.<br />
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 <a href="https://nxttime.wordpress.com/2013/04/19/the-gyro-sensor/">this</a> 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.<br />
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.<br />
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.<br />
I wanted an IMU that combines gyroscope and accelerometer and would be able to read samples faster that 8 ms.<br />
EV3 supports new UART sensors that can support very high communication speed (up to 460 kbit/sec). I have found <a href="https://lejosnews.wordpress.com/2014/09/18/homemade-imu/">an article</a> that can be a good starting point to create own UART sensors for EV3.<br />
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.<br />
During working on the sensors I tested three IMU chips: LSM330DLC, LSM9DS0 and LSM6DS3.<br />
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.<br />
<br />
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.<br />
<h3 style="text-align: left;">
<a name='more'></a><br />Sensor hardware design</h3>
<h3 style="text-align: left;">
</h3>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgil0wTGmYHvx00J6ecTERZCOJp08c3shgDeOs5dFwx-_BIOXR2TTLpIxNufFpIgtd23q8rtpufoGqnCtDu48FwHGf_RPPmzucEWwyVksYbxFKnf2ZUBsLq0gurptlPu2t9HbTI13A-T07S/s1600/lsm330dlc-circuit.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="408" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgil0wTGmYHvx00J6ecTERZCOJp08c3shgDeOs5dFwx-_BIOXR2TTLpIxNufFpIgtd23q8rtpufoGqnCtDu48FwHGf_RPPmzucEWwyVksYbxFKnf2ZUBsLq0gurptlPu2t9HbTI13A-T07S/s640/lsm330dlc-circuit.png" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LSM330DLC sensor schematic</td></tr>
</tbody></table>
The sensor consist of sensor chip, MCU that reads data samples from the sensor chip and sends them to host using EV3 UART protocol, and power converter that converts 5V power supply voltage from EV3 brick to 3.3V that is required for sensor chip and MCU.<br />
I used STM8S103F3U MCU that has very small package that allows me to fit the sensor circuit into required PCB size. PCB has two-layers design.<br />
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiTEhcIjhdH2oUpwp2JPKZmxvIFkHxu0Dl04U39V2c7K_XK7iFPJKs8W2wYqmUMIJmPsm6XsswtP_gW79zxhabmZYfBlxkkIOX5jyYcAPfQ_kIoZk0ppoYPcAZvfSrqgLNyd2HpEkcC8DI5/s1600/lsm330dlc-pcb-top.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiTEhcIjhdH2oUpwp2JPKZmxvIFkHxu0Dl04U39V2c7K_XK7iFPJKs8W2wYqmUMIJmPsm6XsswtP_gW79zxhabmZYfBlxkkIOX5jyYcAPfQ_kIoZk0ppoYPcAZvfSrqgLNyd2HpEkcC8DI5/s640/lsm330dlc-pcb-top.png" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Top PCB view</td></tr>
</tbody></table>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhbYrhf_IMonvZk2i6ceg_8MCSUcwln-zBD8sQTBn1j1mCAetGHEakJn_RZDlnLR5DC-ggWAq7TkfOnv59kQRRAJtfOVPKXK5eWpnm9aSIkFaU4o61xyN0o8lax6vP42GHp43-LOXICjrv6/s1600/lsm330dlc-pcb-bottom.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="304" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhbYrhf_IMonvZk2i6ceg_8MCSUcwln-zBD8sQTBn1j1mCAetGHEakJn_RZDlnLR5DC-ggWAq7TkfOnv59kQRRAJtfOVPKXK5eWpnm9aSIkFaU4o61xyN0o8lax6vP42GHp43-LOXICjrv6/s640/lsm330dlc-pcb-bottom.png" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Bottom PCB view</td></tr>
</tbody></table>
<br />
The first prototype I have made at home, the next ones I have ordered on a PCB fab.<br />
Hardware design for all IMUs can be found in Git <a href="https://github.com/maxmorozov/ev3imu">repository</a>.<br />
I used STM8L Discovery board to debug the sensor software. ST-LINK can be used too.<br />
<div style="text-align: left;">
<br /></div>
<div style="text-align: left;">
Photos of the assembled sensors. <br />
<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhPRAD-EnfXXl7mzZoKCXuIjpMXC-9bOjSVzk2EcOQmyonPd6epA5Pf-FwbkXxFViaTxttQjMmUTys8LKaNIdSowRrCsR55KqFmGJHVVqwz8HZgDZ6vCQl-tGhkr_uS5IQEW2PFWYibq8oE/s1600/DSCN1780.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhPRAD-EnfXXl7mzZoKCXuIjpMXC-9bOjSVzk2EcOQmyonPd6epA5Pf-FwbkXxFViaTxttQjMmUTys8LKaNIdSowRrCsR55KqFmGJHVVqwz8HZgDZ6vCQl-tGhkr_uS5IQEW2PFWYibq8oE/s400/DSCN1780.JPG" width="400" /></a></div>
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhSVoHGsl1g_M5XPv_k_8m9fkHSrt4c1fmgNCddwYnR5ZQOUn9dVhXRtdmYaZlo5OFBULKiZHXOt6NHHsbZWEV60gYm86lFDfmSWJxlvmjE-fP83iVmIuMwFojjvWLmIJ9QYwVk_4MMqsnV/s1600/DSCN1781.JPG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhSVoHGsl1g_M5XPv_k_8m9fkHSrt4c1fmgNCddwYnR5ZQOUn9dVhXRtdmYaZlo5OFBULKiZHXOt6NHHsbZWEV60gYm86lFDfmSWJxlvmjE-fP83iVmIuMwFojjvWLmIJ9QYwVk_4MMqsnV/s400/DSCN1781.JPG" width="400" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LSM330DLC based IMU</td></tr>
</tbody></table>
<div class="separator" style="clear: both; text-align: center;">
<br /></div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjbFN3WlPaJnsN-DRIzUK6HlTrGGfh8xioSIs3LCwBwPVlLdA-jLQCpfsP_E7XqKiYLH9jkPPCHCHBgA919HYOAL38MP0enmJ13q7GIHLM4dSNVbSebJ060AiFyye5p-1ZvSPi9rNFBs_kF/s1600/DSCN1783.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjbFN3WlPaJnsN-DRIzUK6HlTrGGfh8xioSIs3LCwBwPVlLdA-jLQCpfsP_E7XqKiYLH9jkPPCHCHBgA919HYOAL38MP0enmJ13q7GIHLM4dSNVbSebJ060AiFyye5p-1ZvSPi9rNFBs_kF/s400/DSCN1783.JPG" width="400" /></a></div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjEiObusIVjB13yKDAPCdtaBmKFgzPxe7Dh6IzvCVtIvwhV-pY9B3hyICRSbCGyLvZ59pl8vaAJHJmvH4R-4-UpRyJvFnJUVXBMaGdnvQm96FsgDHqvTpgGq7cgEnMY4pn2BA-LtuXmRwFD/s1600/DSCN1785.JPG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjEiObusIVjB13yKDAPCdtaBmKFgzPxe7Dh6IzvCVtIvwhV-pY9B3hyICRSbCGyLvZ59pl8vaAJHJmvH4R-4-UpRyJvFnJUVXBMaGdnvQm96FsgDHqvTpgGq7cgEnMY4pn2BA-LtuXmRwFD/s400/DSCN1785.JPG" width="400" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LSM9DS0 based IMU</td></tr>
</tbody></table>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjbYGgc09PTz7D1yxwA9Oqt8Om_G6ghsi8Bw0C5ap3pDL0QwEcM_iRE3FCM_j2GGPZS9PEDwy0AhfZvIpmUY6mGVuOUeNjU5RC1MO8yHmp1QhqIz4-9QcR7XfEpDccoGBrzsC3AawImzLII/s1600/DSCN1790.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjbYGgc09PTz7D1yxwA9Oqt8Om_G6ghsi8Bw0C5ap3pDL0QwEcM_iRE3FCM_j2GGPZS9PEDwy0AhfZvIpmUY6mGVuOUeNjU5RC1MO8yHmp1QhqIz4-9QcR7XfEpDccoGBrzsC3AawImzLII/s400/DSCN1790.JPG" width="400" /></a></div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi-totJ0aYRMJYyey8O8d59CsN2-FbQUzKiqkBLfLDUDedC1FNXOdBfykYqs0txG_3FWAKMC6Mbl25KugFfBrk3CVKu88zqL-WbXkgcCVBb03KwO4R-CS8Yk5hJpUPNLShduj9T-W44d3VS/s1600/DSCN1791.JPG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="300" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi-totJ0aYRMJYyey8O8d59CsN2-FbQUzKiqkBLfLDUDedC1FNXOdBfykYqs0txG_3FWAKMC6Mbl25KugFfBrk3CVKu88zqL-WbXkgcCVBb03KwO4R-CS8Yk5hJpUPNLShduj9T-W44d3VS/s400/DSCN1791.JPG" width="400" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LSM6DS3 based IMU</td></tr>
</tbody></table>
<br />
<h3 style="text-align: left;">
Sensor software design</h3>
<div>
Sensor software consist of two components: sensor software (firmware) and EV3 driver software. Sensor software is written using C++, EV3 driver is written using Java and can be used from LeJOS EV3 programs. Currently, I don't have enough information to write EV3-G blocks for my IMUs.<br />
<br />
Sensor software is written using Real-Time OS called <a href="https://github.com/scmrtos/scmrtos">scmRTOS</a>. It is written on C++. I used IAR C++ for STM8 version 2.10.5. There is a new version 2.20 but it has problems with code inlining, so I don't recommend it.<br />
<br />
Sensor software supports sending data samples to EV3 host, changing sensor sensitivity (all used sensor chips have multiple full-scale ranges), writing transformation matrix to MCU EEPROM (implemented for LSM6DS3 and LSM9DS0). IMUs can be used in different modes: gyroscope, accelerometer, gyroscope+accelerometer. LSM9DS0 has additional mode - magnetometer. But, I don't have the calibration tool for magnetometer yet.<br />
<br />
The software is designed using C++ templates. The application class can be assembled from modules. The main class is Ev3UartSensor that implements EV3 UART protocol. It has following template parameters:<br />
<br />
<ul style="text-align: left;">
<li>type - sensor type constant to register sensor class in EV3 block.</li>
<li>Uart - UART protocol implementation. Currently I use blocking UART implementation with internal circular input buffer. It is the main reason, why RTOS has been used.</li>
<li>Config - UART config. The sensor starts using the fixed UART speed 2400 bps, than is changed to the necessary communication speed after getting response from the host. The UART config contains necessary information to set up communication parameters.</li>
<li>Device - IMU implementation. This implementation uses 'Curiously recurring template pattern' to allow sensor's core sending samples to the EV3 host</li>
</ul>
<div>
This class is responsible to send device information to EV3, to receive commands from EV3 host and to send sensor samples to EV3. </div>
<div>
IMU implementation, that is passed to Device parameter, is responsible to init sensor chip and to read samples. IMU implementation is template class IMU that has following parameters:</div>
<div>
<ul style="text-align: left;">
<li>ImuCore - device-specific template that contains device communication protocol.</li>
<li>Commands - class that implements commands that EV3 host can send to the sensor.</li>
<li>event_queue_size - size of internal ring buffer that keeps received commands from EV3 host.</li>
<li>EepromWriter - class that provides ability to write data to MCU EEPROM.</li>
<li>Derived - EV3 sensor implementation. Actually, it is Ev3UartSensor.</li>
</ul>
<div>
<br />
For each sensor chip I have prepared appropriate implementation of ImuCore and Commands classes. These implementations are stored in the sensor specific IMU project folder.</div>
</div>
<div>
ImuCore template has following template parameters (LSM6DS3 version):</div>
<div>
<ul style="text-align: left;">
<li>ImuTransport - communication protocol for the sensor chip. Currently it is blocking SPI implementation. The implementation SpiTransport is sensor-specific. It has a parameter that allows to customize address selection format (LSM6DS3 has different address selection algorithm).</li>
<li>SampleProvider - class that reads samples from the sensor and performs necessary data transformations. I have prepared two implementation: SimpleProvider, that provides raw samples without any transformation, and TransformProvider that corrects samples using transformation matrix stored in the MCU EEPROM. The transformation matrix is calculated during sensor's calibration.</li>
<li>Derived - derived class that implements sending samples to EV3 host mechanism. Actually, it is Ev3UartSensor.</li>
</ul>
<div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiiMIuPgBXkQBOr2xGrzNa4Lf-avq5dgNXLDYW0YD8ypLQ5KWUWllvTGKSPiGG4NVOgAQhD2rxXQEkiPkvzVNOLyTykfLD4a1x9o2TTfWtTTYad_7G-WSqLYXD7LeEKkKX3GKDpePF34nlU/s1600/Classes.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiiMIuPgBXkQBOr2xGrzNa4Lf-avq5dgNXLDYW0YD8ypLQ5KWUWllvTGKSPiGG4NVOgAQhD2rxXQEkiPkvzVNOLyTykfLD4a1x9o2TTfWtTTYad_7G-WSqLYXD7LeEKkKX3GKDpePF34nlU/s1600/Classes.png" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;"><span style="font-size: small; text-align: left;">Diagram of assembled IMU class</span></td></tr>
</tbody></table>
LSM330DLC and LSM9DS0 sensors are implemented as composition of two sensors (gyroscope and accelerometer), and have difefrent parameters for accelerometer and gyroscope transports.<br />
<br />
<div style="text-align: left;">
Example of sensor object:</div>
<div style="text-align: left;">
<br /></div>
</div>
<div>
In this example we create an instance of IMU sensor based on LSM6DS3 with communication speed 115200 bps. F_MASER - MCU frequency. It supports sensor calibration and saving calibration matrix into EEPROM. Queue size is 32 for all types. Usage of the same queue size for all classes reduces the generated code size.</div>
<div>
<pre><span style="color: dimgrey;">//---------------------------------------------------------------------------</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">// UART configuration</span>
<span style="color: dimgrey;">//</span>
<span style="color: maroon; font-weight: bold;">typedef</span> UartConfig<span style="color: purple;"><</span>F_MASTER<span style="color: #808030;">,</span> <span style="color: #008c00;">115200</span><span style="color: purple;">></span> uart_config<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Uart<span style="color: purple;"><</span>Uart1<span style="color: #808030;">,</span> <span style="color: #008c00;">32</span><span style="color: purple;">></span> uart_type<span style="color: purple;">;</span>
<span style="color: dimgrey;">//---------------------------------------------------------------------------</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">// IMU class that integrates all devices</span>
<span style="color: dimgrey;">//</span>
<span style="color: maroon; font-weight: bold;">template</span> <span style="color: purple;"><</span><span style="color: maroon; font-weight: bold;">typename</span> Derived<span style="color: purple;">></span>
<span style="color: maroon; font-weight: bold;">struct</span> imu_core_type <span style="color: purple;">:</span> ev3<span style="color: purple;">::</span>lsm6ds3<span style="color: purple;">::</span>ImuCore
<span style="color: #808030;"><</span>
ImuTransport<span style="color: #808030;">,</span>
sensors<span style="color: purple;">::</span>TransformProvider<span style="color: purple;"><</span>eeprom_type<span style="color: #808030;">,</span> eeprom<span style="color: purple;">></span><span style="color: purple;">::</span>Provider<span style="color: #808030;">,</span>
Derived
<span style="color: #808030;">></span> <span style="color: purple;">{</span><span style="color: purple;">}</span><span style="color: purple;">;</span></pre>
<pre><span style="color: maroon; font-weight: bold;">template</span> <span style="color: purple;"><</span><span style="color: maroon; font-weight: bold;">typename</span> Derived<span style="color: purple;">></span>
<span style="color: maroon; font-weight: bold;">struct</span> imu_type <span style="color: purple;">:</span> ev3<span style="color: purple;">::</span>imu<span style="color: purple;">::</span>IMU
<span style="color: #808030;"><</span>
imu_core_type<span style="color: #808030;">,</span>
ev3<span style="color: purple;">::</span>lsm6ds3<span style="color: purple;">::</span>Commands<span style="color: #808030;">,</span>
<span style="color: #008c00;">32</span><span style="color: #808030;">,</span>
ev3<span style="color: purple;">::</span>EepromWriter<span style="color: purple;"><</span>TranformationMatrix<span style="color: purple;">></span><span style="color: #808030;">,</span>
Derived
<span style="color: #808030;">></span> <span style="color: purple;">{</span><span style="color: purple;">}</span><span style="color: purple;">;</span></pre>
<pre><span style="color: purple;">
</span></pre>
<pre style="background-attachment: initial; background-clip: initial; background-image: initial; background-origin: initial; background-position: initial; background-repeat: initial; background-size: initial;"><span style="color: dimgrey;">//---------------------------------------------------------------------------</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">// EV3 UART sensor that provides data to EV3 host</span>
<span style="color: dimgrey;">//</span>
<span style="color: maroon; font-weight: bold;">typedef</span> ev3<span style="color: purple;">::</span>Ev3UartSensor<span style="color: purple;"><</span><span style="color: #008c00;">97</span><span style="color: #808030;">,</span> uart_type<span style="color: #808030;">,</span> uart_config<span style="color: #808030;">,</span> imu_type<span style="color: purple;">></span> sensor_type<span style="color: purple;">;</span></pre>
<pre><span style="color: dimgrey;">// sensor instance</span>
sensor_type sensor;</pre>
</div>
<h3 style="text-align: left;">
Implementation details</h3>
</div>
<div>
STM8 MCU has reserved area for stack. When PUSH command crosses the bottom stack border, the stack pointer sets to the top address of the stack area. This causes problems in multi-stack RTOS like scmRTOS. More details can found <a href="http://wiki.chibios.org/dokuwiki/doku.php?id=chibios:articles:stm8_stack">here</a>. So I place additional stack (process objects in scmRTOS terms) within the stack area using following commands:</div>
<div>
<br />
<div>
CommandHandler commandHandler @ "HW_STACK";</div>
<div>
SensorHandler sensorHandler @ "HW_STACK";</div>
</div>
</div>
<div>
<br /></div>
<div>
HW_STACK area is described in lnkstm8s103f3.icf files that are located in the IMU project folders.</div>
<div>
<br /></div>
<div>
This causes build problems on the latest version of scmRTOM in debug mode. The problem appears after adding some diagnostic code to scmRTOS core. I prepared a custom version of scmRTOS that doesn't have this code. </div>
<div>
<br /></div>
<div>
LSM9DS0 has a dedicated pin to signal that magnetometer sample is ready to read. But I miss this opportunity and this signal is sent using accelerometer data ready line. As a result, the sensor software implements data ready driven behavior in magnetometer-only mode. In combined mode, the magnetometer sample is read when the accelerometer sample is ready. I have prepared the second version of HW design of LSM9DS0 sensor, but have not implemented it yet.</div>
<div>
<br />
Initialization of MCU pins is performed by PortConfigurer template. It has a template parameter, that contains list of pin descriptors. I used C++ template meta-programming to work with list of types that describe MCU pins.<br />
<pre><span style="color: dimgrey;">//---------------------------------------------------------------------------</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">// Hardware initialization section</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">//---------------------------------------------------------------------------</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">// GPIO pins configuration</span>
<span style="color: dimgrey;">//</span>
<span style="color: dimgrey;">//Unused I/O pins are configured as input with internal pull-up resistor.</span>
<span style="color: maroon; font-weight: bold;">typedef</span> input_traits<span style="color: purple;"><</span>PullUp<span style="color: #808030;">,</span> InterruptsDisabled<span style="color: purple;">></span> unused_trait<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_B<span style="color: #808030;">,</span> <span style="color: #008c00;">4</span><span style="color: #808030;">,</span> unused_trait<span style="color: purple;">></span> PinB4<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_B<span style="color: #808030;">,</span> <span style="color: #008c00;">5</span><span style="color: #808030;">,</span> unused_trait<span style="color: purple;">></span> PinB5<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_D<span style="color: #808030;">,</span> <span style="color: #008c00;">3</span><span style="color: #808030;">,</span> unused_trait<span style="color: purple;">></span> PinD3<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_D<span style="color: #808030;">,</span> <span style="color: #008c00;">4</span><span style="color: #808030;">,</span> unused_trait<span style="color: purple;">></span> PinD4<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_C<span style="color: #808030;">,</span> <span style="color: #008c00;">3</span><span style="color: #808030;">,</span> unused_trait<span style="color: purple;">></span> PinC3<span style="color: purple;">;</span>
<span style="color: dimgrey;">//UART pins</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_D<span style="color: #808030;">,</span> <span style="color: #008c00;">5</span><span style="color: #808030;">,</span> output_traits<span style="color: purple;"><</span>PushPull<span style="color: #808030;">,</span> Speed_10Mhz<span style="color: purple;">></span> <span style="color: purple;">></span> PinUartTX<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_D<span style="color: #808030;">,</span> <span style="color: #008c00;">6</span><span style="color: #808030;">,</span> input_traits<span style="color: purple;"><</span>Floating<span style="color: #808030;">,</span> InterruptsDisabled<span style="color: purple;">></span> <span style="color: purple;">></span> PinUartRX<span style="color: purple;">;</span>
<span style="color: dimgrey;">//Chip select pins</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_A<span style="color: #808030;">,</span> <span style="color: #008c00;">3</span><span style="color: #808030;">,</span> output_traits<span style="color: purple;"><</span>PushPull<span style="color: #808030;">,</span> Speed_10Mhz<span style="color: #808030;">,</span> High<span style="color: purple;">></span><span style="color: #808030;">,</span> Low<span style="color: purple;">></span> PinImuSel<span style="color: purple;">;</span>
<span style="color: dimgrey;">//Data ready interrupt pins</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_C<span style="color: #808030;">,</span> <span style="color: #008c00;">4</span><span style="color: #808030;">,</span> input_traits<span style="color: purple;"><</span>Floating<span style="color: #808030;">,</span> RisingEdge<span style="color: purple;">></span> <span style="color: purple;">></span> PinAccelDataReady<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> Pin<span style="color: purple;"><</span>Port_D<span style="color: #808030;">,</span> <span style="color: #008c00;">2</span><span style="color: #808030;">,</span> input_traits<span style="color: purple;"><</span>Floating<span style="color: #808030;">,</span> RisingEdge<span style="color: purple;">></span> <span style="color: purple;">></span> PinGyroDataReady<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">typedef</span> PortConfigurer<span style="color: purple;"><</span>mpl<span style="color: purple;">::</span>make_type_list<span style="color: #808030;"><</span>
PinImuSel<span style="color: #808030;">,</span>
PinAccelDataReady<span style="color: #808030;">,</span> PinGyroDataReady<span style="color: #808030;">,</span>
PinUartTX<span style="color: #808030;">,</span> PinUartRX<span style="color: #808030;">,</span>
PinB4<span style="color: #808030;">,</span> PinB5<span style="color: #808030;">,</span> PinD3<span style="color: #808030;">,</span> PinD4<span style="color: #808030;">,</span> PinC3<span style="color: purple;">></span><span style="color: purple;">::</span>type<span style="color: #808030;">></span> configurer<span style="color: purple;">;</span></pre>
<pre><span style="font-family: "times" , "times new roman" , serif;">
</span></pre>
Port initialization code:
<br />
<pre style="background-attachment: initial; background-clip: initial; background-image: initial; background-origin: initial; background-position: initial; background-repeat: initial; background-size: initial;"><span style="color: dimgrey;">//Initial port configuration</span>
configurer<span style="color: purple;">::</span>configure<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span></pre>
<div>
This code used template metaprograming and code inlining to generate small code to initialize STM8 ports.
</div>
</div>
<pre style="background-attachment: initial; background-clip: initial; background-image: initial; background-origin: initial; background-position: initial; background-repeat: initial; background-size: initial;"></pre>
<pre></pre>
<h3 style="text-align: left;">
EV3 software</h3>
<div>
IMU sensor driver is inherited from UARTSensor class from LeJOS library. Also, it can implement interfaces to select sensor sensitivity and to save transformation matrix to EEPROM.</div>
<div>
<pre><span style="color: maroon; font-weight: bold;">
</span></pre>
<pre><pre style="background-attachment: initial; background-clip: initial; background-image: initial; background-origin: initial; background-position: initial; background-repeat: initial; background-size: initial;"><span style="color: maroon; font-weight: bold;">package</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">sensor</span><span style="color: #808030;">.</span><span style="color: #004a43;">imu</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@author</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">Max Morozov</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">interface</span> ScaleSelector <span style="color: purple;">{</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Changes the gyroscope full</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">scale range</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale range index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the current scale range has been successfully changed</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> setGyroscopeScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Changes the accelerometer full</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">scale range</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale range index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the current scale range has been successfully changed</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> setAccelerometerScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Changes the magnetometer full</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">scale range</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale range index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the current scale range has been successfully changed</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> setMagnetometerScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span></pre>
</pre>
<pre><span style="color: maroon; font-weight: bold;">
</span></pre>
<pre><span style="color: maroon; font-weight: bold;">package</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">sensor</span><span style="color: #808030;">.</span><span style="color: #004a43;">imu</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@author</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">Max Morozov</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">interface</span> ImuEepromWriter <span style="color: purple;">{</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Update accelerometer EEPROM</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> data EEPROM data (4x3 matrix of </span><span style="color: #008c00;">16</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">bit integers)</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the EEPROM data has been successfully written</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> writeAccelerometerEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Update gyroscope EEPROM</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> data EEPROM data (4x3 matrix of </span><span style="color: #008c00;">16</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">bit integers)</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the EEPROM data has been successfully written</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> writeGyroscopeEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Update magnetometer EEPROM</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> data EEPROM data (4x3 matrix of </span><span style="color: #008c00;">16</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">bit integers)</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the EEPROM data has been successfully written</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #bb7977;">boolean</span> writeMagnetomtereEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span></pre>
<pre><span style="color: purple;">
</span></pre>
<div style="text-align: left;">
IMU sensor driver for LSM6DS3 sensor:</div>
<pre><span style="color: purple;">
</span></pre>
<pre><pre style="background-attachment: initial; background-clip: initial; background-image: initial; background-origin: initial; background-position: initial; background-repeat: initial; background-size: initial;"><span style="color: maroon; font-weight: bold;">package</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">sensor</span><span style="color: #808030;">.</span><span style="color: #004a43;">imu</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">port</span><span style="color: #808030;">.</span><span style="color: #004a43;">Port</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">sensor</span><span style="color: #808030;">.</span><span style="color: #004a43;">SensorMode</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">hardware</span><span style="color: #808030;">.</span><span style="color: #004a43;">sensor</span><span style="color: #808030;">.</span><span style="color: #004a43;">UARTSensor</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">robotics</span><span style="color: #808030;">.</span><span style="color: #004a43;">SampleProvider</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> lejos</span><span style="color: #808030;">.</span><span style="color: #004a43;">utility</span><span style="color: #808030;">.</span><span style="color: #004a43;">Delay</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> java</span><span style="color: #808030;">.</span><span style="color: #004a43;">nio</span><span style="color: #808030;">.</span><span style="color: #004a43;">ByteBuffer</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">import</span><span style="color: #004a43;"> java</span><span style="color: #808030;">.</span><span style="color: #004a43;">nio</span><span style="color: #808030;">.</span><span style="color: #004a43;">ByteOrder</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">class</span> ImuLsm6ds3 <span style="color: maroon; font-weight: bold;">extends</span> UARTSensor <span style="color: maroon; font-weight: bold;">implements</span> ImuEepromWriter, ScaleSelector <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">long</span> SWITCHDELAY <span style="color: #808030;">=</span> <span style="color: #008c00;">200</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">long</span> SCALE_SWITCH_DELAY <span style="color: #808030;">=</span> <span style="color: #008c00;">10</span><span style="color: purple;">;</span>
<span style="color: dimgrey;">//Return device to the state right after power on</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> DEVICE_RESET <span style="color: #808030;">=</span> <span style="color: green;">0x11</span><span style="color: purple;">;</span>
<span style="color: dimgrey;">//Accelerometer sensitivity</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> ACC_SCALE_2G <span style="color: #808030;">=</span> <span style="color: green;">0x20</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> ACC_SCALE_4G <span style="color: #808030;">=</span> <span style="color: green;">0x21</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> ACC_SCALE_8G <span style="color: #808030;">=</span> <span style="color: green;">0x22</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> ACC_SCALE_16G <span style="color: #808030;">=</span> <span style="color: green;">0x23</span><span style="color: purple;">;</span>
<span style="color: dimgrey;">//Gyroscope sensitivity</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> GYRO_SCALE_245DPS <span style="color: #808030;">=</span> <span style="color: green;">0x30</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> GYRO_SCALE_500DPS <span style="color: #808030;">=</span> <span style="color: green;">0x31</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> GYRO_SCALE_1000DPS <span style="color: #808030;">=</span> <span style="color: green;">0x32</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> GYRO_SCALE_2000DPS <span style="color: #808030;">=</span> <span style="color: green;">0x33</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> GYRO_SCALE_125DPS <span style="color: #808030;">=</span> <span style="color: green;">0x34</span><span style="color: purple;">;</span>
<span style="color: dimgrey;">//Write calibration matrix into EEPROM</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_ACC_2G <span style="color: #808030;">=</span> <span style="color: green;">0x40</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_ACC_4G <span style="color: #808030;">=</span> <span style="color: green;">0x41</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_ACC_8G <span style="color: #808030;">=</span> <span style="color: green;">0x42</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_ACC_16G <span style="color: #808030;">=</span> <span style="color: green;">0x43</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_GYRO_245DPS <span style="color: #808030;">=</span> <span style="color: green;">0x50</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_GYRO_500DPS <span style="color: #808030;">=</span> <span style="color: green;">0x51</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_GYRO_1000DPS <span style="color: #808030;">=</span> <span style="color: green;">0x52</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_GYRO_2000DPS <span style="color: #808030;">=</span> <span style="color: green;">0x53</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">byte</span> CALIBRATE_GYRO_125DPS <span style="color: #808030;">=</span> <span style="color: green;">0x54</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">int</span> ACCEL_SCALE <span style="color: #808030;">=</span> <span style="color: #bb7977; font-weight: bold;">Short</span><span style="color: #808030;">.</span>MAX_VALUE <span style="color: #808030;">+</span> <span style="color: #008c00;">1</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">float</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> gyroScale <span style="color: #808030;">=</span> <span style="color: purple;">{</span>
<span style="color: green;"> 8.75e-3</span><span style="color: #006600;">f</span><span style="color: #808030;">,</span>
<span style="color: green;"> 17.5e-3</span><span style="color: #006600;">f</span><span style="color: #808030;">,</span>
<span style="color: green;"> 35e-3</span><span style="color: #006600;">f</span><span style="color: #808030;">,</span>
<span style="color: green;"> 70e-3</span><span style="color: #006600;">f</span><span style="color: #808030;">,</span>
<span style="color: green;"> 4.375e-3</span><span style="color: #006600;">f</span><span style="color: purple;">}</span><span style="color: purple;">;</span><span style="color: dimgrey;">//in degree per second / digit</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">static</span> <span style="color: maroon; font-weight: bold;">final</span> <span style="color: #bb7977;">float</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> accelScale <span style="color: #808030;">=</span> <span style="color: purple;">{</span>
2f <span style="color: #808030;">/</span> ACCEL_SCALE<span style="color: #808030;">,</span>
4f <span style="color: #808030;">/</span> ACCEL_SCALE<span style="color: #808030;">,</span>
8f <span style="color: #808030;">/</span> ACCEL_SCALE <span style="color: #808030;">,</span>
16f <span style="color: #808030;">/</span> ACCEL_SCALE<span style="color: purple;">}</span><span style="color: purple;">;</span> <span style="color: dimgrey;">//in g / digit</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">boolean</span> rawMode<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> ImuLsm6ds3<span style="color: #808030;">(</span>Port port<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">(</span>port<span style="color: #808030;">,</span> <span style="color: maroon; font-weight: bold;">false</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> ImuLsm6ds3<span style="color: #808030;">(</span>Port port<span style="color: #808030;">,</span> <span style="color: #bb7977;">boolean</span> rawMode<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">super</span><span style="color: #808030;">(</span>port<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>rawMode <span style="color: #808030;">=</span> rawMode<span style="color: purple;">;</span>
setModes<span style="color: #808030;">(</span><span style="color: maroon; font-weight: bold;">new</span> SensorMode<span style="color: #808030;">[</span><span style="color: #808030;">]</span><span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;"> new</span> CombinedMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">,</span>
<span style="color: maroon; font-weight: bold;"> new</span> AccelerationMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">,</span>
<span style="color: maroon; font-weight: bold;"> new</span> GyroMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">}</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> reset<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: #bb7977;">byte</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> buffer <span style="color: #808030;">=</span> <span style="color: maroon; font-weight: bold;">new</span> <span style="color: #bb7977;">byte</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> <span style="color: purple;">{</span>DEVICE_RESET<span style="color: purple;">}</span><span style="color: purple;">;</span>
port<span style="color: #808030;">.</span>write<span style="color: #808030;">(</span>buffer<span style="color: #808030;">,</span> <span style="color: #008c00;">0</span><span style="color: #808030;">,</span> buffer<span style="color: #808030;">.</span>length<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> setGyroscopeScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>scaleNo <span style="color: #808030;">></span><span style="color: #808030;">=</span> <span style="color: #008c00;">0</span> <span style="color: #808030;">&</span><span style="color: #808030;">&</span> scaleNo <span style="color: #808030;"><</span> <span style="color: #008c00;">5</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
ImuSensorMode mode <span style="color: #808030;">=</span> <span style="color: #808030;">(</span>ImuSensorMode<span style="color: #808030;">)</span> getMode<span style="color: #808030;">(</span>getCurrentMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
mode<span style="color: #808030;">.</span>setGyroScale<span style="color: #808030;">(</span>getGyroScale<span style="color: #808030;">(</span>scaleNo<span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">return</span> setScale<span style="color: #808030;">(</span>GYRO_SCALE_245DPS <span style="color: #808030;">+</span> scaleNo<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: maroon; font-weight: bold;">false</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> setAccelerometerScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>scaleNo <span style="color: #808030;">></span><span style="color: #808030;">=</span><span style="color: #008c00;">0</span> <span style="color: #808030;">&</span><span style="color: #808030;">&</span> scaleNo <span style="color: #808030;"><</span> <span style="color: #008c00;">4</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
ImuSensorMode mode <span style="color: #808030;">=</span> <span style="color: #808030;">(</span>ImuSensorMode<span style="color: #808030;">)</span>getMode<span style="color: #808030;">(</span>getCurrentMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
mode<span style="color: #808030;">.</span>setAccelScale<span style="color: #808030;">(</span>getAccelScale<span style="color: #808030;">(</span>scaleNo<span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">return</span> setScale<span style="color: #808030;">(</span>ACC_SCALE_2G <span style="color: #808030;">+</span> scaleNo<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: maroon; font-weight: bold;">false</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> setMagnetometerScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: maroon; font-weight: bold;">false</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> writeAccelerometerEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> writeEeprom<span style="color: #808030;">(</span>CALIBRATE_ACC_2G <span style="color: #808030;">+</span> scaleNo<span style="color: #808030;">,</span> data<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> writeGyroscopeEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> writeEeprom<span style="color: #808030;">(</span>CALIBRATE_GYRO_245DPS <span style="color: #808030;">+</span> scaleNo<span style="color: #808030;">,</span> data<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Update magnetometer EEPROM</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> scaleNo scale index</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> data EEPROM data (4x3 matrix of </span><span style="color: #008c00;">16</span><span style="color: #7f9fbf; font-weight: bold;">-</span><span style="color: #3f5fbf;">bit integers)</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@return</span><span style="color: #3f5fbf;"> true if the EEPROM data has been successfully written</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">boolean</span> writeMagnetomtereEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: maroon; font-weight: bold;">false</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">boolean</span> writeEeprom<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> writeCommand<span style="color: #808030;">,</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> data<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
ByteBuffer command <span style="color: #808030;">=</span> ByteBuffer<span style="color: #808030;">.</span>allocate<span style="color: #808030;">(</span>data<span style="color: #808030;">.</span>length <span style="color: #808030;">*</span> <span style="color: #008c00;">2</span> <span style="color: #808030;">+</span> <span style="color: #008c00;">1</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
command<span style="color: #808030;">.</span>order<span style="color: #808030;">(</span>ByteOrder<span style="color: #808030;">.</span>LITTLE_ENDIAN<span style="color: #808030;">)</span><span style="color: purple;">;</span>
command<span style="color: #808030;">.</span>put<span style="color: #808030;">(</span><span style="color: #808030;">(</span><span style="color: #bb7977;">byte</span><span style="color: #808030;">)</span> writeCommand<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">for</span> <span style="color: #808030;">(</span><span style="color: #bb7977;">short</span> aData <span style="color: #808030;">:</span> data<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
command<span style="color: #808030;">.</span>putShort<span style="color: #808030;">(</span>aData<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #bb7977;">byte</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> array <span style="color: #808030;">=</span> command<span style="color: #808030;">.</span>array<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #bb7977;">boolean</span> success <span style="color: #808030;">=</span> port<span style="color: #808030;">.</span>write<span style="color: #808030;">(</span>array<span style="color: #808030;">,</span> <span style="color: #008c00;">0</span><span style="color: #808030;">,</span> array<span style="color: #808030;">.</span>length<span style="color: #808030;">)</span> <span style="color: #808030;">=</span><span style="color: #808030;">=</span> array<span style="color: #808030;">.</span>length<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>success<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: dimgrey;">//Wait for writing the data to EEPROM</span>
Delay<span style="color: #808030;">.</span>msDelay<span style="color: #808030;">(</span>array<span style="color: #808030;">.</span>length <span style="color: #808030;">*</span> <span style="color: #008c00;">3</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">return</span> success<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">boolean</span> setScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleCommand<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: #bb7977;">byte</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> buffer <span style="color: #808030;">=</span> <span style="color: maroon; font-weight: bold;">new</span> <span style="color: #bb7977;">byte</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> <span style="color: purple;">{</span><span style="color: #808030;">(</span><span style="color: #bb7977;">byte</span><span style="color: #808030;">)</span>scaleCommand<span style="color: purple;">}</span><span style="color: purple;">;</span>
<span style="color: #bb7977;">boolean</span> success <span style="color: #808030;">=</span> port<span style="color: #808030;">.</span>write<span style="color: #808030;">(</span>buffer<span style="color: #808030;">,</span> <span style="color: #008c00;">0</span><span style="color: #808030;">,</span> buffer<span style="color: #808030;">.</span>length<span style="color: #808030;">)</span> <span style="color: #808030;">=</span><span style="color: #808030;">=</span> buffer<span style="color: #808030;">.</span>length<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>success<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
Delay<span style="color: #808030;">.</span>msDelay<span style="color: #808030;">(</span>SCALE_SWITCH_DELAY<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">return</span> success<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">float</span> getAccelScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>rawMode<span style="color: #808030;">)</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">1</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">else</span>
<span style="color: maroon; font-weight: bold;">return</span> accelScale<span style="color: #808030;">[</span>scaleNo<span style="color: #808030;">]</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">float</span> getGyroScale<span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> scaleNo<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">if</span> <span style="color: #808030;">(</span>rawMode<span style="color: #808030;">)</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">1</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">else</span>
<span style="color: maroon; font-weight: bold;">return</span> gyroScale<span style="color: #808030;">[</span>scaleNo<span style="color: #808030;">]</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> SampleProvider getCombinedMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> getMode<span style="color: #808030;">(</span><span style="color: #008c00;">0</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> SampleProvider getAccelerationMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> getMode<span style="color: #808030;">(</span><span style="color: #008c00;">1</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> SampleProvider getGyroMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> getMode<span style="color: #808030;">(</span><span style="color: #008c00;">2</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">class</span> CombinedMode <span style="color: maroon; font-weight: bold;">extends</span> BaseSensorMode <span style="color: purple;">{</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">6</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977; font-weight: bold;">String</span> getName<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #0000e6;">"ALL"</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> getMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">0</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setGyroScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">for</span> <span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> i <span style="color: #808030;">=</span> <span style="color: #008c00;">3</span><span style="color: purple;">;</span> i <span style="color: #808030;"><</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span> <span style="color: #808030;">+</span><span style="color: #808030;">+</span>i<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>scale<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span> <span style="color: #808030;">=</span> scale<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setAccelScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">for</span> <span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> i <span style="color: #808030;">=</span> <span style="color: #008c00;">0</span><span style="color: purple;">;</span> i <span style="color: #808030;"><</span> <span style="color: #008c00;">3</span><span style="color: purple;">;</span> <span style="color: #808030;">+</span><span style="color: #808030;">+</span>i<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>scale<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span> <span style="color: #808030;">=</span> scale<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">class</span> AccelerationMode <span style="color: maroon; font-weight: bold;">extends</span> BaseSensorMode <span style="color: purple;">{</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">3</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977; font-weight: bold;">String</span> getName<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #0000e6;">"Acceleration"</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> getMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">1</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setAccelScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">for</span> <span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> i <span style="color: #808030;">=</span> <span style="color: #008c00;">0</span><span style="color: purple;">;</span> i <span style="color: #808030;"><</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span> <span style="color: #808030;">+</span><span style="color: #808030;">+</span>i<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>scale<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span> <span style="color: #808030;">=</span> scale<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: maroon; font-weight: bold;">class</span> GyroMode <span style="color: maroon; font-weight: bold;">extends</span> BaseSensorMode <span style="color: purple;">{</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">3</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977; font-weight: bold;">String</span> getName<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #0000e6;">"Rate"</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">int</span> getMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">return</span> <span style="color: #008c00;">2</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setGyroScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">for</span> <span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> i <span style="color: #808030;">=</span> <span style="color: #008c00;">0</span><span style="color: purple;">;</span> i <span style="color: #808030;"><</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span> <span style="color: #808030;">+</span><span style="color: #808030;">+</span>i<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>scale<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span> <span style="color: #808030;">=</span> scale<span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">abstract</span> <span style="color: maroon; font-weight: bold;">class</span> BaseSensorMode <span style="color: maroon; font-weight: bold;">implements</span> ImuSensorMode <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">protected</span> <span style="color: #bb7977;">float</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> scale<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">private</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> buffer<span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">public</span> BaseSensorMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span> <span style="color: purple;">{</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>scale <span style="color: #808030;">=</span> <span style="color: maroon; font-weight: bold;">new</span> <span style="color: #bb7977;">float</span><span style="color: #808030;">[</span>sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">]</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">this</span><span style="color: #808030;">.</span>buffer <span style="color: #808030;">=</span> <span style="color: maroon; font-weight: bold;">new</span> <span style="color: #bb7977;">short</span><span style="color: #808030;">[</span>sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">]</span><span style="color: purple;">;</span>
setAccelScale<span style="color: #808030;">(</span>getAccelScale<span style="color: #808030;">(</span><span style="color: #008c00;">0</span><span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
setGyroScale<span style="color: #808030;">(</span>getGyroScale<span style="color: #808030;">(</span><span style="color: #008c00;">0</span><span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: #3f5fbf;">/**</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> Fetches a sample from a sensor or filter.</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"></span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> sample The array to store the sample in.</span>
<span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">*</span><span style="color: #3f5fbf;"> </span><span style="color: #7f9fbf; font-weight: bold;">@param</span><span style="color: #3f5fbf;"> offset</span>
<span style="color: #3f5fbf;"> */</span>
<span style="color: #808030;">@</span>Override
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> fetchSample<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span><span style="color: #808030;">[</span><span style="color: #808030;">]</span> sample<span style="color: #808030;">,</span> <span style="color: #bb7977;">int</span> offset<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
switchMode<span style="color: #808030;">(</span>getMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">,</span> SWITCHDELAY<span style="color: #808030;">)</span><span style="color: purple;">;</span>
port<span style="color: #808030;">.</span>getShorts<span style="color: #808030;">(</span>buffer<span style="color: #808030;">,</span> <span style="color: #008c00;">0</span><span style="color: #808030;">,</span> sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: maroon; font-weight: bold;">for</span><span style="color: #808030;">(</span><span style="color: #bb7977;">int</span> i<span style="color: #808030;">=</span><span style="color: #008c00;">0</span><span style="color: purple;">;</span>i<span style="color: #808030;"><</span>sampleSize<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span><span style="color: #808030;">+</span><span style="color: #808030;">+</span>i<span style="color: #808030;">)</span> <span style="color: purple;">{</span>
sample<span style="color: #808030;">[</span>offset <span style="color: #808030;">+</span> i<span style="color: #808030;">]</span> <span style="color: #808030;">=</span> buffer<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span> <span style="color: #808030;">*</span> scale<span style="color: #808030;">[</span>i<span style="color: #808030;">]</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setGyroScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span> <span style="color: purple;">}.</span>
<span style="color: maroon; font-weight: bold;">public</span> <span style="color: #bb7977;">void</span> setAccelScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span> <span style="color: purple;">{</span> <span style="color: purple;">}</span>
<span style="color: purple;">}</span>
<span style="color: maroon; font-weight: bold;">interface</span> ImuSensorMode <span style="color: maroon; font-weight: bold;">extends</span> SensorMode <span style="color: purple;">{</span>
<span style="color: #bb7977;">int</span> getMode<span style="color: #808030;">(</span><span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #bb7977;">void</span> setGyroScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: #bb7977;">void</span> setAccelScale<span style="color: #808030;">(</span><span style="color: #bb7977;">float</span> scale<span style="color: #808030;">)</span><span style="color: purple;">;</span>
<span style="color: purple;">}</span>
<span style="color: purple;">}</span></pre>
</pre>
</div>
<h3 style="text-align: left;">
Calibration software</h3>
<div>
To calibrate accelerometer and gyroscope sensors I used algorithm described in <a href="chrome-extension://oemmndcbldboiebfnladdacbdfmadadm/http://www.st.com/resource/en/application_note/cd00269797.pdf">this application note</a>.</div>
<div>
I have written a program to measure sensor raw data at 6 stationary positions and then calculate transformation matrix. For gyroscope sensor calibration I used a vinyl disk player turntable to provide fixed and stable angular velocity. The player has minimal rotation speed about 200 dps, so I couldn't calibrate LSM6DS3 rotation scale 125 dps. But I found that matrix for this scale can be computed from the matrix for scale 250 dps with scaled offset values because other elements of the transformation matrix are the same for different scales (gyroscope only).</div>
<div>
The calibration software is run on EV3 block. It measures a number of raw samples at 6 positions, then used least square method to calculate transformation matrix. After that it writes the transformation matrix to the sensor.</div>
<div>
Calibration software can be found in Git <a href="https://github.com/maxmorozov/ev3imu">repository</a>. </div>
<div>
<br /></div>
<div>
The calibration software for the magnetometer is not completed yet.</div>
<div>
<h3 style="text-align: left;">
<br />The source code</h3>
</div>
<div>
Git repository <a href="https://github.com/maxmorozov/ev3imu">https://github.com/maxmorozov/ev3imu</a> contains the hardware design in DipTrace format, source code for sensor firmware, driver for LeJOS EV3 and calibration software.</div>
<div>
<br /></div>
<div>
<br /></div>
</div>
Maximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.com0tag:blogger.com,1999:blog-729812208796274999.post-2289909897268606982015-03-23T12:16:00.002+03:002023-11-14T22:50:55.760+03:00Mathematical Model of Lego EV3 Motor<div dir="ltr" style="text-align: left;" trbidi="on">
<div dir="ltr" style="text-align: left;" trbidi="on">
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.<br />
<div dir="ltr" style="text-align: left;" trbidi="on">
<div dir="ltr" style="text-align: left;" trbidi="on">
<div>
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.</div>
<a name='more'></a>Motor's mathematical model can be described by following equations:<br />
<div>
<br />
$$\begin{equation}<br />
\left\{\begin{aligned}<br />
\displaystyle \dot\omega&=\frac{K_\tau\cdot{I}-B\cdot\omega-A_r-\tau_d}{J}\\<br />
\displaystyle \dot{I}&=\frac{U-R_a\cdot{I}-K_b\cdot{\omega}}{L_a}<br />
\end{aligned}\right.<br />
\label{eq:eq1}<br />
\end{equation}$$<br />
where:<br />
<table border="1" cellpadding="0" cellspacing="0"><tbody>
<tr><td valign="top" width="54"><i>I<br />U</i></td><td valign="top" width="95">(A)<br />
(V)</td><td valign="top" width="236">armature current<br />
armature voltage</td></tr>
<tr><td valign="top" width="54">ω<br />
τ<sub>d</sub></td><td valign="top" width="95">(rad/sec)<br />
(N·m)</td><td valign="top" width="236">shaft's angular velocity<br />
shaft's load torque</td></tr>
<tr><td valign="top" width="54"><i>L<sub>a</sub><br />R<sub>a</sub></i><br />
<i>K</i><sub>τ</sub><i><br />K<sub>b</sub></i></td><td valign="top" width="95">(H)<br />
(Ω)<br />
(N·m/A)<br />
(V/(rad/s))</td><td valign="top" width="236">armature inductance<br />
armature resistance<br />
torque constant<br />
back electromotive force coefficient</td></tr>
<tr><td valign="top" width="54"><i>J</i><br />
<i>B</i><br />
<i>A<sub>r</sub></i><i></i></td><td valign="top" width="95">(kg·m²)<br />
(N·m/(rad/s))<br />
(N·m)</td><td valign="top" width="236">rotor's moment of inertia<br />
viscosity resistance coefficient<br />
dry friction force</td></tr>
</tbody></table>
<br />
Some of these parameters can be measured directly. I could measure \(L_a\)<i> </i>and \(R_a\)<i> </i>using RLC meter.<br />
\(R_a\) = 4.196 Ohm<br />
\(L_a\) = 4.94 mH<br />
But \(R_a\)<i> </i>in the equation \eqref{eq:eq1} includes the full circuit resistance, for example internal resistance of batteries. It must be calculated from other measurements.<br />
<br />
We will use following scenarios to calculate motor's parameters:<br />
<ol style="text-align: left;">
<li>Analyzing deceleration curve, when the motor rotates freely without external load and stops due to internal friction. This measurement allow us to find relations between \(A_r, B\) and \(J\).</li>
<li>Measure rotation speed and the motor's current under load. In this case the motor rotates with a constant speed. This measurement allows us to estimate \(B, K_b, R_a\) and \(K_\tau\). </li>
<li>Analyzing acceleration curve, when the motor starts rotating without external load. This measurement allows us to adjust parameters calculated on the previous step.</li>
</ol>
<h3 style="text-align: left;">
1. Deceleration curve </h3>
<div style="text-align: left;">
In this case the motor circuit is open, external load is missing, so the motor model becomes simpler (\(I=0, \tau_d=0\)):<br />
$$\begin{equation}<br />
\left\{\begin{aligned}<br />
\displaystyle \dot\omega&=\frac{-B\cdot\omega-A_r}{J}\\<br />
\displaystyle U&=K_b\cdot{\omega}<br />
\end{aligned}\right.<br />
\label{eq:eq2}<br />
\end{equation}$$<br />
The second expression allows us to find \(K_b\) if we know rotation speed. But in this case we cannot measure rotation speed and we will use only the first equation: </div>
<div style="text-align: left;">
$$\displaystyle \dot\omega=\frac{-B\cdot\omega-A_r}{J}$$</div>
<div style="text-align: left;">
The solution of this equation can be found using the initial conditions (\(\omega(0)=\omega_0\)):</div>
<div style="text-align: left;">
$$\omega(t) = -\frac{A_r}{B}+ e^{\textstyle-\frac{B \cdot t}{J}}(\omega_0+\frac{A_r}{B})$$<br />
Because we can measure motor position but not the rotation speed, we need to find motor position function. Integration of rotation speed function with initial condition \(\phi(0)=0\) gives us the following expression:<br />
$$\begin{equation}<br />
\phi(t) = -\frac{A_r\cdot t}{B}+ (1-e^{\textstyle-\frac{B \cdot t}{J}})(\omega_0+\frac{A_r}{B})\frac{J}{B}<br />
\label{eq:eq_phi}<br />
\end{equation}$$<br />
Now we can use least square method to find constants, but some of them are depended on others.<br />
So we should introduce additional independent constants:<br />
$$\left\{\begin{aligned}<br />
\displaystyle T_1&=\frac{B}{J}\\<br />
\displaystyle T_2&=\frac{A_r}{B}<br />
\end{aligned}\right.$$<br />
After that we can rewrite the function \(\phi(t)\) from \eqref{eq:eq_phi}:<br />
\[\phi(t)=-T_2\cdot t+\frac{\omega0+T_2}{T_1}(1-e^{\textstyle-T_1 \cdot t})\] <br />
I wrote a program that starts the motor and turning the power off after one second of rotation. It measures the motor position every 5 ms and writes them to a <a href="https://drive.google.com/file/d/0B6p_C0b8jM_ydFIzODJtbEtqU0k/view?usp=sharing&resourcekey=0-QSnPSfULbpcGx_AY2ujICg" rel="nofollow">log-file</a>. Then a wrote a SciLab <a href="https://drive.google.com/file/d/0B6p_C0b8jM_yYW9SQTR6a3pnaXM/view?usp=sharing&resourcekey=0-2WAuSEsgbaFiE089LtXNjQ" rel="nofollow">script</a> that uses Least Squares method to find dependencies between \(A_r, B\) and \(J\). The calculation results are following relations:<br />
$$\begin{equation}<br />
\frac{B}{J}=0.4837581433546762\\<br />
\frac{A_r}{B}=10.697523425732065<br />
\label{eq:eq_decel1}<br />
\end{equation}$$<br />
and graph that shows measured and calculated motor position.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhBrXBJ2o2B6MlUv2H7hJLdzaONEh7fsqRSnGL8JUfdzX7TTIgypRM2KnUeaE25OyU3tgAIT2Mu_g9DRXZOrB-w5kuHuPzmnDDoIqIBiCfo04C_g9e3GXqNRx-zcXsnPAFg4fVPGjZdqTqa/s1600/graph1.gif" style="margin-left: auto; margin-right: auto;"><img border="0" height="453" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhBrXBJ2o2B6MlUv2H7hJLdzaONEh7fsqRSnGL8JUfdzX7TTIgypRM2KnUeaE25OyU3tgAIT2Mu_g9DRXZOrB-w5kuHuPzmnDDoIqIBiCfo04C_g9e3GXqNRx-zcXsnPAFg4fVPGjZdqTqa/w640-h453/graph1.gif" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Motor position graph: measured and calculated values</td></tr>
</tbody></table>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
Now we have relations between \(A_r, B\) and \(J\), and we can use them in the next steps.</div>
<div style="text-align: left;">
</div>
<h3 style="text-align: left;">
2. Measurement under load </h3>
In case that the motor is steady state (\(\omega=const\) and \(I=const\)), then these differential equations are represented by the following linear equations:<br />
$$\begin{equation}<br />
\left\{\begin{aligned}<br />
\displaystyle &K_\tau\cdot{I}-B\cdot\omega=A_r+\tau_d\\<br />
\displaystyle &R_a\cdot{I}+K_b\cdot{\omega}=U<br />
\end{aligned}\right.<br />
\label{eq:eq_load}<br />
\end{equation}$$<br />
The solution of this system can be found in form:<br />
$$\left\{\begin{aligned}<br />
I &= \frac{U\cdot B + K_b (A_r+\tau_d)}{R_a \cdot B + K_b \cdot K_\tau}\\<br />
\omega &= \frac{R_a (A_r+\tau_d) - K_\tau \cdot U}{R_a \cdot B + K_b \cdot K_\tau}<br />
\end{aligned}\right.$$<br />
We can measure \(I, \omega, U\) and \(\tau_d\). Both equations are linear and have \(\tau_d\) as an argument. So, if we know \(A_r\) we can find \(B, R_a, K_\tau, K_b\) using results of measurements in two points.<br />
These measurement gives us three parameters for each point: (\(\tau_{d_1}, I_1, \omega_1\)) and (\(\tau_{d_2}, I_2, \omega_2\)).<br />
The system that contains results of two measurements:<br />
$$\left\{\begin{aligned}<br />
I_1 &= \frac{U\cdot B + K_b (A_r+\tau_{d_1})}{R_a \cdot B + K_b \cdot K_\tau}\\<br />
\omega_1 &= \frac{R_a (A_r+\tau_{d_1}) - K_\tau \cdot U}{R_a \cdot B + K_b \cdot K_\tau}\\<br />
I_2 &= \frac{U\cdot B + K_b (A_r+\tau_{d_2})}{R_a \cdot B + K_b \cdot K_\tau}\\<br />
\omega_2 &= \frac{R_a (A_r+\tau_{d_2}) - K_\tau \cdot U}{R_a \cdot B + K_b \cdot K_\tau}<br />
\end{aligned}\right.$$</div>
<div>
Solution of this system can be found in following form:<br />
$$\begin{equation}<br />
\left\{\begin{aligned}<br />
B &= \frac{I_1 (A_r+\tau_{d_2}) - I_2 (A_r+\tau_{d_1}) }{\omega_2\cdot I_1-\omega_1\cdot I_2}\\<br />
K_b &= \frac{U(I_1- I_2) }{\omega_2\cdot I_1-\omega_1\cdot I_2}\\<br />
R_a &= \frac{U(\omega_1- \omega_2) }{\omega_2\cdot I_1-\omega_1\cdot I_2}\\<br />
K_\tau &= \frac{\omega_1 (A_r+\tau_{d_2}) - \omega_2 (A_r+\tau_{d_1}) }{\omega_2\cdot I_1-\omega_1\cdot I_2}<br />
\end{aligned}\right.<br />
\label{eq:eq_load2}<br />
\end{equation}$$<br />
<br />
<h4 style="text-align: left;">
Measurement assembly</h4>
</div>
<div>
I used a special assembly (LDD <a href="https://drive.google.com/file/d/0B6p_C0b8jM_ydVdzNXo0X1hZakE/view?usp=sharing&resourcekey=0-xkyGsGat7qGIUn_D8qeqvA">model</a>) to measure rotation speed and the current under load. The assembly lifts objects with known weight and recorded the motor positions. From these positions I calculate the rotation speed. Also I used a circuit to measure the motor current using a multimeter. </div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEin3Ugh2bn73vJTNKRTdyE_7czQ9V8vUROHTxqHtwS2410IlClTL5luh_JkXsu9YJRYyyh_37_n4pIKlWaWr-Iz66nLhc4vYgBOp_3uZq3kw5ZarOKb_yGCan4MX-irMMsDgTHhmO2_zFs5/s1600/Assembly2.png" style="margin-left: auto; margin-right: auto;"><img border="0" height="408" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEin3Ugh2bn73vJTNKRTdyE_7czQ9V8vUROHTxqHtwS2410IlClTL5luh_JkXsu9YJRYyyh_37_n4pIKlWaWr-Iz66nLhc4vYgBOp_3uZq3kw5ZarOKb_yGCan4MX-irMMsDgTHhmO2_zFs5/s1600/Assembly2.png" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rendered assembly</td></tr>
</tbody></table>
<div>
The real assembly photo</div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgLpJxTpeS4NZR8b7ykDo2qUh_dHU4cPVSoB99uIKRMxEnrMvtceM9zy4EhyNyVoaMsYzNGNldITo-mRKj0Qnm58Y9sNJUZT7TpEq999B72DZvj3ZGRAe8rCJziB9NysossPQdteonI_-tI/s1600/DSC_0061.JPG" style="margin-left: auto; margin-right: auto;"><img border="0" height="360" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgLpJxTpeS4NZR8b7ykDo2qUh_dHU4cPVSoB99uIKRMxEnrMvtceM9zy4EhyNyVoaMsYzNGNldITo-mRKj0Qnm58Y9sNJUZT7TpEq999B72DZvj3ZGRAe8rCJziB9NysossPQdteonI_-tI/s1600/DSC_0061.JPG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">NXT based assembly</td></tr>
</tbody></table>
<div>
This photo was taken on the first attempt of EV3 motor parameters measurement, when I used NXT brick with LeJOS to record motor positions because NXT LeJOS is some kind of "real-time" OS. But LeJOS for EV3 uses embedded Java that requires some time to warm-up and does not allow me to organize stable and precise measurement time intervals. Unfortunately, I found that EV3 and NXT have different electrical parameters so I had to repeat my measurements on EV3 using C++ instead of Java for my program.</div>
<div>
<br /></div>
<div>
I used a wheel from Lego 8051 MotorBike set. The groove when I put the thread has diameter 68 mm. I used scales to measure weight of lifted objects. Then the torque can be calculated:<br />
$$\tau_d=g \cdot m\cdot r $$<br />
Where \(g\) is gravitational acceleration, \(m\) is object mass, and \(r\) is the wheel radius.<br />
<br /></div>
<h4 style="text-align: left;">
The measurement results</h4>
<div>
I made a number of measurements and put the results into a table:</div>
<style type="text/css">.nobrtable br { display: none } tr {text-align: right;}</style>
<br />
<div class="nobrtable">
<table border="1" cellpadding="3" cellspacing="0" style="width: 100%px;">
<tbody>
<tr>
<th>Load torque,<br />
N*cm</th>
<th>Motor current,<br />
A</th>
<th>Rotation speed,<br />
rad/sec</th>
</tr>
<tr>
<td>0.00</td>
<td>0.054</td>
<td>15.8825</td>
</tr>
<tr>
<td>6.04</td>
<td>0.24</td>
<td>13.1598</td>
</tr>
<tr>
<td>9.74</td>
<td>0.36</td>
<td>11.5017</td>
</tr>
<tr>
<td>15.28</td>
<td>0.54</td>
<td>9.0583</td>
</tr>
<tr>
<td>19.01</td>
<td>0.66</td>
<td>7.1035</td>
</tr>
</tbody></table>
</div>
<br />
The graphical representation of measured results:<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhWga8zn4wp_GU5_LjD0uR0KcC4782J7yIfD7z-GvPdet0kooHEeXThhvFlF_8ZCqeDOl-1LqnvtF3xE5_79dkqItUyTeRPZnJHjgrI2Tnr7I4xcTMWeiOqB40Ln8nLLFg29dQxoyiHxRBp/s1600/Untitled.png" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="408" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhWga8zn4wp_GU5_LjD0uR0KcC4782J7yIfD7z-GvPdet0kooHEeXThhvFlF_8ZCqeDOl-1LqnvtF3xE5_79dkqItUyTeRPZnJHjgrI2Tnr7I4xcTMWeiOqB40Ln8nLLFg29dQxoyiHxRBp/s1600/Untitled.png" width="640" /></a></div>
<br />
<h4 style="text-align: left;">
Motor parameters estimation</h4>
<div>
Now we can find the motor parameters. We will use the first and the last row of the table with<br />
measurement results. So:<br />
$$\left\{\begin{aligned}<br />
\tau_{d_1}&=0 ~ N\cdot m\\<br />
I_1&=0.054 ~ A\\<br />
\omega_1&=15.8825 ~ rad/sec<br />
\end{aligned}\right.$$<br />
$$\left\{\begin{aligned}<br />
\tau_{d_2}&=0.1901 ~ N\cdot m\\<br />
I_2&=0.66 ~ A\\<br />
\omega_2&=7.1035 ~ rad/sec<br />
\end{aligned}\right.$$<br />
Using these reference data we can find the solution of the system \eqref{eq:eq_load2}:<br />
$$\left\{\begin{aligned}<br />
B&=-0.06000677881 \cdot A_r+0.001016586247 \\<br />
K_b &= 0.4716532815\\<br />
R_a &= 6.832750917\\<br />
K_\tau &= 0.8693067325 \cdot A_r+0.2989986520<br />
\end{aligned}\right.$$<br />
Using the expression for B and the value for T2 found during the deceleration curve measurement \eqref{eq:eq_decel1} we can find \(A_r\):<br />
$$-0.06000677881 \cdot A_r+0.001016586247=\frac{A_r}{10.697523425732065}$$<br />
From this equation we can find \(A_r=0.006623300293\). Then we can calculate other parameters:<br />
$$\begin{equation}<br />
\begin{aligned}<br />
B&=0.0006191433314 \\<br />
K_b &= 0.4716532815\\<br />
R_a &= 6.832750917\\<br />
K_\tau &= 0.3047563315<br />
\end{aligned}<br />
\label{eq:eq_step2_params}<br />
\end{equation}$$<br />
<div>
Because this method has relatively low accuracy (because I used a simple multimeter), I used acceleration curve measurement to find motor parameters with better precision.<br />
<br />
<h4 style="text-align: left;">
3. Acceleration curve</h4>
</div>
</div>
<div>
In this case we will find the solution of the motor equation system \eqref{eq:eq1} for \(\omega(t)\). It has following form:<br />
$$\begin{equation}<br />
\omega(t)=C_1 e^{\textstyle (\varkappa_1+\varkappa_3)t} + C_2 e^{\textstyle (\varkappa_1-\varkappa_3)t}+\frac{K_\tau U-R_a (A_r+\tau_d)}{C_5}<br />
\label{eq:eq_omega}<br />
\end{equation}$$<br />
where<br />
$$<br />
C_1=\frac{C_4}{C_5}(\frac{\varkappa_2}{\varkappa_3} - 1) (\varkappa_1 - \varkappa_3)\\<br />
C_2= - \frac{C_3}{C_5}(\frac{\varkappa_2}{\varkappa_3} + 1) (\varkappa_1 + \varkappa_3)\\<br />
C_3=\frac{L_a}{2}(A_r + \tau_d + \frac{J U}{K_b}(\varkappa_2 - \varkappa_3))\\<br />
C_4=\frac{L_a}{2}(A_r + \tau_d+ \frac{J U}{K_b}(\varkappa_2 + \varkappa_3))\\<br />
C_5=B R_a + K_b K_\tau<br />
$$<br />
and where<br />
$$\varkappa_1 = -\frac{B L_a + J R_a} {2 J L_a}\\<br />
\varkappa_2 = \frac{B L_a - J R_a} {2 J L_a}\\<br />
\varkappa_3 = \sqrt{\varkappa_2^2 - \frac{K_b K_\tau}{J L_a}}$$
</div>
<div>
Because we can get only motor rotation angle from the motor's encoder, we should find expression for \(\phi(t)\) by integrating \eqref{eq:eq_omega} with respect of initial condition \(\phi(0) = 0\):<br />
$$<br />
\phi(t)=C_6 e^{\textstyle (\varkappa_1+\varkappa_3)t} + C_7 e^{\textstyle (\varkappa_1-\varkappa_3)t}+\frac{(K_\tau U-R_a (A_r+\tau_d))t}{C_5}-(C_6+C_7)<br />
$$<br />
where<br />
$$<br />
C_6=\frac{C_1}{\varkappa_1+\varkappa_3}\\<br />
C_7=\frac{C_2}{\varkappa_1-\varkappa_3}<br />
$$<br />
I used the SciLab <a href="https://drive.google.com/file/d/0B6p_C0b8jM_yQWJtR0RRZS1TeEU/view?usp=sharing&resourcekey=0-DTJn4fv_0Y-cFbrJC5KxDQ">script</a> to calculate motor parameters. It uses values \eqref{eq:eq_step2_params} as initial values and uses \eqref{eq:eq_decel1} to calculate \(J\) and \(A_r\). The source <a href="https://drive.google.com/file/d/0B6p_C0b8jM_ydFIzODJtbEtqU0k/view?usp=sharing&resourcekey=0-QSnPSfULbpcGx_AY2ujICg">data</a> is the same as for deceleration curve measurement. The script produces following motor parameters:<br />
$$\begin{equation}<br />
\begin{aligned}<br />
R_a&=6.832749059810827\\<br />
B&=0.000726962269165\\<br />
K_\tau&=0.304766706036738\\<br />
K_b&=0.459965726538748\\<br />
J&=0.001502739083882\\<br />
A_r&=0.007776695904018\\<br />
L_a&=0.00494<br />
\end{aligned}<br />
\label{eq:eq_step3_params}<br />
\end{equation}$$
<br />
The comparison of experimental and calculated motor positions:<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjOhptQZS2gPkXGg6j4oA2ccL4SSbyrVpMdhPI32dnDPTYYqotFvZPeg50p4WiiLKkmyd3EHE_YB0BzVQmgfz_si5LsjRW9YU1K71Cj4shfbu2To4_n6-Hw8qrL-KzvbWcLAeOW1qDr11Oq/s1600/%D0%93%D1%80%D0%B0%D1%84%D0%B8%D1%87%D0%B5%D1%81%D0%BA%D0%BE%D0%B5+%D0%BE%D0%BA%D0%BD%D0%BE+0.gif" style="margin-left: auto; margin-right: auto;"><img border="0" height="460" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjOhptQZS2gPkXGg6j4oA2ccL4SSbyrVpMdhPI32dnDPTYYqotFvZPeg50p4WiiLKkmyd3EHE_YB0BzVQmgfz_si5LsjRW9YU1K71Cj4shfbu2To4_n6-Hw8qrL-KzvbWcLAeOW1qDr11Oq/s1600/%D0%93%D1%80%D0%B0%D1%84%D0%B8%D1%87%D0%B5%D1%81%D0%BA%D0%BE%D0%B5+%D0%BE%D0%BA%D0%BD%D0%BE+0.gif" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Experimental and calculated motor angle</td></tr>
</tbody></table>
<h4 style="text-align: left;">
Conclusion</h4>
<div>
The mathematical model of Lego EV3 motor allows us to calculate controllers using modern techniques like <a href="http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs--self-balancing-two-wheeled-robot--controller-design">this</a> and check them in a virtual environment. </div>
<div>
<br /></div>
</div>
</div>
</div>
</div>
</div>
Maximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.com7tag:blogger.com,1999:blog-729812208796274999.post-15947193987506317312012-10-04T14:48:00.000+04:002013-04-23T21:26:39.977+04:00Laser Sensor for Lego NXT<div dir="ltr" style="text-align: left;" trbidi="on">
This sensor was made for experiments with triangular localization.<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh4b_lWvGFWLNauMaq9-GiikW8ZrptTGkHY9nSc86JmfKQr3x1G_vfSi5aOEKCOMdujtModdo3opUmxOtVDysr1HzkPw-AuHdiEckDtT4xIXmk24W41pFv2zlqWADKfhKHAkNJnyipidzvj/s1600/IMG_9149.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh4b_lWvGFWLNauMaq9-GiikW8ZrptTGkHY9nSc86JmfKQr3x1G_vfSi5aOEKCOMdujtModdo3opUmxOtVDysr1HzkPw-AuHdiEckDtT4xIXmk24W41pFv2zlqWADKfhKHAkNJnyipidzvj/s320/IMG_9149.JPG" width="320" /></a></div>
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.<br />
<br />
<a name='more'></a><br />
I was impressed by this video and wanted to reproduce this experiment.<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
<iframe allowfullscreen='allowfullscreen' webkitallowfullscreen='webkitallowfullscreen' mozallowfullscreen='mozallowfullscreen' width='320' height='266' src='https://www.youtube.com/embed/IMI63k5W3sU?feature=player_embedded' frameborder='0'></iframe></div>
<br />
I tried to make the laser sensor by replacing the LED of the Lego light sensor with the <a href="http://dx.com/p/6mm-5mw-red-laser-module-3-5-4-5v-13378">red laser</a> I had bought on DealExtreme.<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiD4dQrU9oyR5XNyPb8iKF6FihtFwbqo0M_-G11M9h4RZlkCMSghE6x8T-FPWxpWp8YavTGvfAS0jftUMpZ6fP9YUZ2Ce2ZYX46FqmXJZVuFE7h8EluT8mEb5JUwBXJW3NBEsnLCTYRXVHX/s1600/Sensor1.png" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiD4dQrU9oyR5XNyPb8iKF6FihtFwbqo0M_-G11M9h4RZlkCMSghE6x8T-FPWxpWp8YavTGvfAS0jftUMpZ6fP9YUZ2Ce2ZYX46FqmXJZVuFE7h8EluT8mEb5JUwBXJW3NBEsnLCTYRXVHX/s320/Sensor1.png" width="320" /></a></div>
<br />
I made beacons using <a href="http://www.giolite.com/eng/theory/theory_03.asp">GIO-Lite</a><span class="serif"><span class="block"> Reflective Material. <span class="serif"><span class="block"> It is composed of wide angle, exposed retro-flective lenses
bonded to the durable cloth. <span class="serif"><span class="block">The GIO-Lite Tape is 50 mm width and can be easily glued.</span></span></span></span></span></span><br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<img border="0" height="270" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiP7yOKOF492H5mT_1FHEp7GYMS4NW1SyIh-aDZIK9lc4zjDg9U9xSjalLFRmt2PUMt0zjonWiVn2lMw9wEZ-k158T1RtKeI9bzHm6jJTrVzWdm3NG0za60ipztv1HJ9iNULemENEaAeXGg/s320/Target.png" width="320" /></div>
<br />
The sensor worked, but its sensitivity was not enough for triangulation. It could detect reflected light on distance about 1 meter. Also it was sensitive to ambient light. This problem with ambient light could be solved by using modulation of the laser beam, but the <span class="active">sensor's short-sight</span> problem was still there.<br />
<br />
I found the <a href="http://www.philohome.com/sensors/lasersensor.htm">Philo's Laser Sensor</a> that worked well on long distances and filtered out the ambient light. It contained a lens to concentrate reflected light and a photo IC to modulate laser beam and detect the reflected light. But his design was for Lego RCX. I decided to make NXT laser sensor using <a href="http://www.junun.org/MarkIII/Info.jsp?item=79">Hamamatsu S6986 Proximity Sensor </a> (also may be found <a href="http://www.active-robots.com/hamamatsu-proximity-detector.html">here</a>) and <a href="http://dx.com/p/12-5mm-mini-secondary-optics-plastic-20-pack-4588">plastic lens</a>. Also I bought the <a href="http://mindsensors.com/index.php?module=pagemaster&PAGE_user_op=view_page&PAGE_id=139&MMN_position=50:50">NXT socket</a>. <br />
<br />
<h3 style="text-align: left;">
Circuit design</h3>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiGs1rdElf7aW99yPCfQxyBLtlj8AMOJoL3o8-NxuRf4Gk2ugiNXIYwaJ3BZlQDf7myR-kRUYMoop7zv-2AW-o_S-8XulGk58CROq2NXyflqvjTJ91PbK6mlxnDiK1gKmhT9V0ZvFWGLi5Y/s1600/Circuit2.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="352" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiGs1rdElf7aW99yPCfQxyBLtlj8AMOJoL3o8-NxuRf4Gk2ugiNXIYwaJ3BZlQDf7myR-kRUYMoop7zv-2AW-o_S-8XulGk58CROq2NXyflqvjTJ91PbK6mlxnDiK1gKmhT9V0ZvFWGLi5Y/s640/Circuit2.PNG" width="640" /></a></div>
<br />
The S6986 works as a current generator for the LED but the value of its current can vary from 15 to 60 mA. The maximal current of 5 mW lasers usually is lower than 60 mA, typical value is about 35 mA. So it's important to check the output current of the particular photo IC before mounting it to PCB to make sure that the laser will not be damaged.<br />
S6986 generates impulse current so you need to use an oscilloscope or a peak level detector to measure the cathode current.<br />
If the measured current exceeds the maximal current of the laser diode, you can use the current adjustment scheme, described in the <a href="https://docs.google.com/open?id=0B6p_C0b8jM_yQU85YlQtTWZXYzQ">S6986 datasheet</a>. This is more preferable than using a pull up resistor, because this scheme does not block the integrated current generator of S6986.<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhcdPisy5HZrR7YKTx0EmrnUhfiJqxD7RVGKQTWf-yJxEYjRxm7wmRGbnUMHpVD0sKiE5GjzcIN-p_-Ww36enGw5tKNFA2FQY6m6ZQf0TI8NL9og7o9B-fx5fwKdDbl1h7A2yybVD6XR2Sj/s1600/Adjustment.png" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="320" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhcdPisy5HZrR7YKTx0EmrnUhfiJqxD7RVGKQTWf-yJxEYjRxm7wmRGbnUMHpVD0sKiE5GjzcIN-p_-Ww36enGw5tKNFA2FQY6m6ZQf0TI8NL9og7o9B-fx5fwKdDbl1h7A2yybVD6XR2Sj/s320/Adjustment.png" width="284" /></a></div>
<br />
I don't have an oscilloscope and I've used the peak detector circuit to measure the voltage on precise resistors installed instead of the laser module.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg3Ll62Rd-tP4M9acNrZl6AZfl1KwpIN0Wsf4L1DOEwuRSpCxh_i0a2A8uW9oM36Mld1VvYZi_A-YOrYndY1hcXO5wen2ulAzFByDEzPD2vbJ2rnFxTMBhGKSngCw8JiasEMedljNh3orbH/s1600/PeakDetector.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg3Ll62Rd-tP4M9acNrZl6AZfl1KwpIN0Wsf4L1DOEwuRSpCxh_i0a2A8uW9oM36Mld1VvYZi_A-YOrYndY1hcXO5wen2ulAzFByDEzPD2vbJ2rnFxTMBhGKSngCw8JiasEMedljNh3orbH/s320/PeakDetector.png" width="320" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Peak level detector</td></tr>
</tbody></table>
<br />
I measured the cathode current of my chips and found that it was about 25
mA. My laser modules had 82 Ohm pull up resistor to limit the current. I
measured the current through the laser module when it was connected to 4 and 2 pins of
the NXT socket (NXT produces a stabilized voltage 4.75V on these pins)
and found that the current was 33 mA. I did not know the maximal current of my laser module, but its description contained information about working voltage (3.5 - 4.5V). Because the cathode current did not exceed 33 mA, I decided that it would be safe to
remove the pull up resistor and connect the laser diode to the circuit
directly.<br />
<br />
<h3 style="text-align: left;">
</h3>
<h3 style="text-align: left;">
PCB design (view from top)</h3>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhc9xEj3yn7OMD4zie2sZ8TD34y9_gmo_94ZMO-SuuH_rjlC97a3WIyV8BiLmh8PgHJ5x_QWRQCiJ3Dd6AdvN1fOMU1Ejj25u9fYoLZykD_6KsQZZKFRBD_1KblGl_CAGoxj1ZY4RflQ2vX/s1600/PCB.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="264" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhc9xEj3yn7OMD4zie2sZ8TD34y9_gmo_94ZMO-SuuH_rjlC97a3WIyV8BiLmh8PgHJ5x_QWRQCiJ3Dd6AdvN1fOMU1Ejj25u9fYoLZykD_6KsQZZKFRBD_1KblGl_CAGoxj1ZY4RflQ2vX/s640/PCB.PNG" width="640" /></a></div>
<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<br />
I used SMD components to fit the circuit on the bottom side of the sensor's PCB. I used resistors with 0805 form-factor. The PCB size was 39.25 x 16.7 mm. I used a laser printer to print <a href="https://docs.google.com/open?id=0B6p_C0b8jM_ydmhpWFpfdTAxWFU">PCB layout</a> and a hot iron to transfer the printed image to the bottom copper layer of the interface board. Then I used Ferric Chloride (FeCl3) to remove uncovered parts of the copper layer.<br />
<br />
After mounting electronic components on the PCB I formed contacts of the S6986 to place it into the lens focus. I measured the lens focus distance and found that it was about 10 mm.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhFALsA75bRojd15IRnMOffegBcBoDPqa1Bc79GnKAva0qXH0E2GfKo2oLEgJJtcVKqH6qYAWxtIByMnOP5X1u8VTRTqPeJszEmGldOoUzT71ELS-iAeWl6NsunUUGggvLpcsCB4z9kDVvF/s1600/Scheme.png" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="250" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhFALsA75bRojd15IRnMOffegBcBoDPqa1Bc79GnKAva0qXH0E2GfKo2oLEgJJtcVKqH6qYAWxtIByMnOP5X1u8VTRTqPeJszEmGldOoUzT71ELS-iAeWl6NsunUUGggvLpcsCB4z9kDVvF/s640/Scheme.png" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">This diagram shows the main components of the sensor and how they are mechanically disposed.</td></tr>
</tbody></table>
<br />
<br />
<br />
<h3 style="text-align: left;">
Photos of assembled circuit</h3>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgF5gV0DkTcTR-lhtm9iPITm61KIib2hhLIFGlJVvj0b6ccUuLjwUX0ExPDrMnPTvYlXdIaYJYuJKkxH1eEQlPLrcPAyQckinFlYNdFlBRukyGTWNjG1XAlLjMysGrSUYA9iLRJDweP_E2G/s1600/IMG_9133.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgF5gV0DkTcTR-lhtm9iPITm61KIib2hhLIFGlJVvj0b6ccUuLjwUX0ExPDrMnPTvYlXdIaYJYuJKkxH1eEQlPLrcPAyQckinFlYNdFlBRukyGTWNjG1XAlLjMysGrSUYA9iLRJDweP_E2G/s320/IMG_9133.JPG" width="320" /></a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEizgKn_82k_vjdHM-Rc2djHT6lmCsMsZOAr1_jHwRC-AVE0pfnj4JTle12hbpqavoLydSIImdFImKsBMZcCGq4bx6h2BVlDTq0xlVCiAaFrkFpmi5NqHIAs8a1rn0vTRrDAIDUZCzWt6tgo/s1600/IMG_9134.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEizgKn_82k_vjdHM-Rc2djHT6lmCsMsZOAr1_jHwRC-AVE0pfnj4JTle12hbpqavoLydSIImdFImKsBMZcCGq4bx6h2BVlDTq0xlVCiAaFrkFpmi5NqHIAs8a1rn0vTRrDAIDUZCzWt6tgo/s320/IMG_9134.JPG" width="320" /></a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiu9QGRb2jBzHYg9xGTQTT_-fz2u5sJdIXgoGkrsNxtQ312Fqsu0QJAawGQsOLUASbaEpBp-V4voaFT7v-OjMPGDpZu-lAYCjEHX-FVEcvVv348CZofbCMrstuLZj2wNjaBSJ2nBMpjsVzA/s1600/IMG_9135.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiu9QGRb2jBzHYg9xGTQTT_-fz2u5sJdIXgoGkrsNxtQ312Fqsu0QJAawGQsOLUASbaEpBp-V4voaFT7v-OjMPGDpZu-lAYCjEHX-FVEcvVv348CZofbCMrstuLZj2wNjaBSJ2nBMpjsVzA/s320/IMG_9135.JPG" width="320" /></a></div>
<br />
The next step was to make optical system.I needed to mount the lens.<br />
<br />
But the first I needed to disassemble the sensor case.<br />
<br />
I made holes in the top white part of the sensor to get access to the side locks.<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiwBV0r_guXe3k4Zd4phDX8jBkLyUKwUqfZUJmtyq9gKksJXwGHOEimQwhF1I8j8mk589hMUg3kywY9az2kw1oghrT03Qjof3srPLEpVEJWOuVWDQGlxs2dEtbG6D-ww0i-q1CteQhhM_fi/s1600/IMG_9138.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiwBV0r_guXe3k4Zd4phDX8jBkLyUKwUqfZUJmtyq9gKksJXwGHOEimQwhF1I8j8mk589hMUg3kywY9az2kw1oghrT03Qjof3srPLEpVEJWOuVWDQGlxs2dEtbG6D-ww0i-q1CteQhhM_fi/s320/IMG_9138.JPG" width="320" /></a></div>
<br />
The other way to disassemble the sensor - <a href="http://www.kevincook.net/NXTHack/step%203.htm">to slice the white plastic tabs</a>.<br />
<br />
<br />
Then I modified the Lego light sensor case - made holes for the laser module and the lens. It was a hard thing because of complex shape of the light sensor's case.<br />
<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj1Sw9mJUwZR96HkyWQK6M5ysHe1kE661o8BfdegrVGI3hOqcUwaSQTV2gDxix8telB-XuLU86jR9ZnoTGTOA-_pVMSq2I_Q0ombjLNN_uS1z_FcFJ7TQ_ELGdFJhyTwclFsBQy1GmnMgkX/s1600/IMG_9136.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj1Sw9mJUwZR96HkyWQK6M5ysHe1kE661o8BfdegrVGI3hOqcUwaSQTV2gDxix8telB-XuLU86jR9ZnoTGTOA-_pVMSq2I_Q0ombjLNN_uS1z_FcFJ7TQ_ELGdFJhyTwclFsBQy1GmnMgkX/s320/IMG_9136.JPG" width="320" /></a></div>
<br />
Assembled sensor images<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgelsRGTacrDxzvPj768wTzy4L7LbR22kin9iLuMTDisHC1a0suSQvl5ZtDQVhRDtp_HpjOcAcknO2SvQlFKL8q_F1wg40My3GO4VMy2ENvun5JmlqpM3gTjqp2REJLuwe2mgUvnM_OaLei/s1600/IMG_9140.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgelsRGTacrDxzvPj768wTzy4L7LbR22kin9iLuMTDisHC1a0suSQvl5ZtDQVhRDtp_HpjOcAcknO2SvQlFKL8q_F1wg40My3GO4VMy2ENvun5JmlqpM3gTjqp2REJLuwe2mgUvnM_OaLei/s320/IMG_9140.JPG" width="320" /></a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj4MqROmjDdTH1Cs_cybBBEPfPiTIfnEHESOShmUDXJfvNphy0WE_9OQjmbk3YahHCMlf9fm3iVgHLKHWwdB8qrFDUi9PT7eqKL2zTrU4LG0j6zDG51H2tpGdx60DWX5e7NqfKUMqYa8ltd/s1600/IMG_9142.JPG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" height="240" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj4MqROmjDdTH1Cs_cybBBEPfPiTIfnEHESOShmUDXJfvNphy0WE_9OQjmbk3YahHCMlf9fm3iVgHLKHWwdB8qrFDUi9PT7eqKL2zTrU4LG0j6zDG51H2tpGdx60DWX5e7NqfKUMqYa8ltd/s320/IMG_9142.JPG" width="320" /></a></div>
<br />
The sensor detects usual objects up to 30 cm. The target covered by GIO-Lite or <span class="serif"><span class="block">3M Scotchlite reflective materials</span></span> can be detected at distance up to 10 meters (probably more, but I did not tested it yet).<br />
<br />
The sensor can be controlled by Lego LightSensor NXT-G block. Raw value 1023 means that the sensor is detecting the target. Low raw value (~90) indicates that the sensor is not detecting the target.<br />
<br />
I use LeJOS, and I've tested the laser sensor using the LightSensor class. In this case sensor's output value about 90 (raw value ~95) means "no target", value "0" (raw value 1023) means "target detected".<br />
<br />
<br />
<br />
<br /></div>
Maximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.com7tag:blogger.com,1999:blog-729812208796274999.post-90293773492074466152011-01-14T17:15:00.021+03:002012-10-05T17:22:07.895+04:00Motor controller with feed-forward for Lego NXT<div dir="ltr" style="text-align: left;" trbidi="on">
<div class="separator" style="clear: both; text-align: center;">
</div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi97kfIQO70d8gENmqKCQ62Tet8-P9xEcypqQJ7UVu2jiv5SZFmps8MxpM0KfSt4jUIb3QqYjybdUkNEJdVzgnXl8XrgfuO9eKj7i6mYdn90KfrxaO8z7j3e6kEJy_l-aI2dBXvjcoqcera/s1600/rotate-400-ff.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"></a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgB0h0aFqeYz47OlNDueXN070sz-sZdOgUw5AX57DMA-GBm62vyvVqrwPpTFzZcD7XNqtZxvJSFXe_VFiTSaqa4mMRfv8-SvZeEqsgHELzPLWOQ7p_78FEmaRnJGDMr2IkHHgfii8kkHiBK/s1600/equation2.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"></a></div>
<span style="font-size: large;">Goals </span><br />
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.<br />
<br />
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. <br />
<br />
<a name='more'></a><br />
To implement smooth acceleration, I used motion profile generator that calculated the next position at each regulation cycle and PID regulator that should keep the specified position. This regulator works well when the set point changes slowly. But when we use smooth acceleration, the set point is changed each regulation cycle and the regulator has some constant delay in reaching the target position. As as result, the motor comes in the target rotation position with non-zero speed and spends some time for oscillations around the target point. This inaccuracy could be reduced by using smaller discretization periods, but I did not want to do that because I wanted to decrease CPU load.<br />
<br />
Then I decided to extend my controller with feed-forward that should calculate the base power that need to ensure rotation to the specified angle. In this case PID regulator should only compensate the impact of the external load.<br />
<br />
<span style="font-size: large;">PID Controller With Motion Profile Generator</span><br />
Because Lego controller is focused on speed regulation, it cannot provide good precision in case when we need to reach a particular position and keep it. To provide such ability in my motor controller I decided to use PID regulator that focused on position regulation. To provide speed regulation (especially with smooth acceleration) I implemented motion profile generator. It calculates the new position each regulation cycle. This approach allows me to implement different acceleration algorithms using different motion profile generators. I implemented two generators. Using them, the user can turn smooth acceleration on and off.<br />
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiD_8Cj871ArZWdcfRhX4UiD-MNVWbusgA_cNTBti7VS_bcnjB40xFCSNXMYMa5AOp_85EQHZondsJY6m8Zq5AaXslI5yFZes2Gwtp6EDGRWGcqAFnHunXPpR-ycSsWRJdXrEk3E8jV4gfM/s1600/motion-profile1.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="339" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiD_8Cj871ArZWdcfRhX4UiD-MNVWbusgA_cNTBti7VS_bcnjB40xFCSNXMYMa5AOp_85EQHZondsJY6m8Zq5AaXslI5yFZes2Gwtp6EDGRWGcqAFnHunXPpR-ycSsWRJdXrEk3E8jV4gfM/s640/motion-profile1.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiD_8Cj871ArZWdcfRhX4UiD-MNVWbusgA_cNTBti7VS_bcnjB40xFCSNXMYMa5AOp_85EQHZondsJY6m8Zq5AaXslI5yFZes2Gwtp6EDGRWGcqAFnHunXPpR-ycSsWRJdXrEk3E8jV4gfM/s640/motion-profile1.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Example of generated motion profile with smooth acceleration</td></tr>
</tbody></table>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiBfvpMWo-lOBRae7uj0AIExpreoK3QwXbkAscmtz4YAj_umlPaZIrO5mSRHOHC-b7qECy_ftD-mUKrFfb_l_UwHZp7wX6XB2cwkOsRtNdguG1DISiWQCn2sgU2nO9GRryOlJIUr4jJowA9/s1600/motion-profile2.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="372" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiBfvpMWo-lOBRae7uj0AIExpreoK3QwXbkAscmtz4YAj_umlPaZIrO5mSRHOHC-b7qECy_ftD-mUKrFfb_l_UwHZp7wX6XB2cwkOsRtNdguG1DISiWQCn2sgU2nO9GRryOlJIUr4jJowA9/s640/motion-profile2.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiBfvpMWo-lOBRae7uj0AIExpreoK3QwXbkAscmtz4YAj_umlPaZIrO5mSRHOHC-b7qECy_ftD-mUKrFfb_l_UwHZp7wX6XB2cwkOsRtNdguG1DISiWQCn2sgU2nO9GRryOlJIUr4jJowA9/s640/motion-profile2.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Example of generated motion profile without smooth acceleration</td></tr>
</tbody></table>
I tested version that used integer arithmetic and found that additional multiplications and divisions reduced performance. In fact, the motion profile generator that uses floating point works faster than the generator that uses integer arithmetic. I think this happens because the LeJOS executes Java byte-code using an interpreter.<br />
<br />
<span style="font-size: large;">Using Feed-forward to Improve Response to Command Signals</span><br />
To use feed forward we should implement the system with two degrees of freedom.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEggpdpInH0-jgZqPuB2K8DvG-2h8lKBOjVV6EIztcdNnUWmnF3QL962cxdBtKOUJcFc3Hu0jKHLvOuRfR6GdPvk-cJHGvKbXkBq_PXZiUmZDliWZQPp3VW4NHXzt9WcRB1MWnzT6pUCBPj_/s1600/ff-controller.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="230" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEggpdpInH0-jgZqPuB2K8DvG-2h8lKBOjVV6EIztcdNnUWmnF3QL962cxdBtKOUJcFc3Hu0jKHLvOuRfR6GdPvk-cJHGvKbXkBq_PXZiUmZDliWZQPp3VW4NHXzt9WcRB1MWnzT6pUCBPj_/s640/ff-controller.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEggpdpInH0-jgZqPuB2K8DvG-2h8lKBOjVV6EIztcdNnUWmnF3QL962cxdBtKOUJcFc3Hu0jKHLvOuRfR6GdPvk-cJHGvKbXkBq_PXZiUmZDliWZQPp3VW4NHXzt9WcRB1MWnzT6pUCBPj_/s640/ff-controller.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">The system with two degrees of freedom</td></tr>
</tbody></table>
On this diagram <i>u<sub>c</sub></i><span style="font-size: xx-small;"> <span style="font-size: small;">is a position value that comes from the motion profile generator. </span></span><i>H<sub>c</sub></i><span style="font-size: xx-small;"><span style="font-size: small;"> is our PID regulator it generates feed-back output, </span></span><i>H<sub>p</sub></i><span style="font-size: xx-small;"><span style="font-size: small;"> is the motor, <i>y</i> is the tachometer value. </span></span><i>H<sub>m</sub></i><span style="font-size: xx-small;"><span style="font-size: small;"> transfers the new position value to regulator. The most interesting part is </span></span><i>H<sub>u</sub></i><span style="font-size: xx-small;"><span style="font-size: small;"> that generates feed-forward. This module should calculate the power that should be sent to the motor to ensure rotation to the specified position. In other words this module should implement inversion of transfer function of our motor.</span></span><br />
<br />
<br />
<span style="font-size: large;">Finding the inverse of the motor model</span><br />
To find inverse of motor model we should find the motor model itself. I used following differential equations to describe mechanical and electrical state of DC motor:<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjZRLlzs1cZmw5PQpcS9VqB8cF1kyWoTqNEZXoI6a9puAYxJRJAjnH_CUo0jYqmf3X0KJo94eGOpqAaYHLF81AIChioJOObR4voq_SMGQt4SWRqZSdQep9m5LNt7hsoZBORmaDG5vFF3YOG/s1600/equation.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjZRLlzs1cZmw5PQpcS9VqB8cF1kyWoTqNEZXoI6a9puAYxJRJAjnH_CUo0jYqmf3X0KJo94eGOpqAaYHLF81AIChioJOObR4voq_SMGQt4SWRqZSdQep9m5LNt7hsoZBORmaDG5vFF3YOG/s1600/equation.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjZRLlzs1cZmw5PQpcS9VqB8cF1kyWoTqNEZXoI6a9puAYxJRJAjnH_CUo0jYqmf3X0KJo94eGOpqAaYHLF81AIChioJOObR4voq_SMGQt4SWRqZSdQep9m5LNt7hsoZBORmaDG5vFF3YOG/s1600/equation.PNG" /></a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgNCIGvSlBjm2_oecIXgZxMUW_d0x4FTwCITA7qo2mHHz71WvoN9tJrYZSzYawE2LpiokAo7c_lWwnBUlsADBBVS63ws1qnn7M3osDyX7skR-OGxweCfpES4J-Y-reAFY58E53R0LlBLNDC/s1600/equation.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><br />
</a></div>
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEicNKlUiVdAPqjybR3aUVNtbU5sbotilRAqakLiYPKXIMiVMpGQZkt0UEPpoJQ7mlhvfU83cNWsB_tQDl3tbh_m4k0GNxw6wGppyV9Y6xC1ifegriebhH2zrFB-GTxH1m6EO9AsuGTXaF5q/s1600/equation.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"> </a></div>
<br />
where:<br />
<table border="1" cellpadding="0" cellspacing="0"><tbody>
<tr><td valign="top" width="54"><i>i<br />
e</i></td><td valign="top" width="95">(A)<br />
(V)</td><td valign="top" width="236">armature current<br />
armature voltage</td></tr>
<tr><td valign="top" width="54">ω<br />
τ<sub>d</sub></td><td valign="top" width="95">(rad/sec)<br />
(N·m)</td><td valign="top" width="236">shaft's angular velocity<br />
shaft's load torque</td></tr>
<tr><td valign="top" width="54"><i>L<sub>a</sub><br />
R<sub>a</sub></i><br />
<i>K</i><sub>τ</sub><i><br />
K<sub>b</sub></i></td><td valign="top" width="95">(H)<br />
(Ω)<br />
(N·m/A)<br />
(V/(rad/s))</td><td valign="top" width="236">armature inductance<br />
armature resistance<br />
torque constant<br />
back electromotive force coefficient</td></tr>
<tr><td valign="top" width="54"><i>J</i><br />
<i>B</i><br />
<i>A<sub>r</sub></i><i> </i></td><td valign="top" width="95">(kg·m²)<br />
(N·m/(rad/s))<br />
(N·m)</td><td valign="top" width="236">rotor's moment of inertia<br />
viscosity resistance coefficient<br />
dry friction force</td> </tr>
</tbody></table>
<br />
As I described in my previous post, I calculated these constants from measurements of motor under load performed by <a href="http://www.philohome.com/motors/motorcomp.htm">Philo</a> and used them to emulate the NXT motor:<br />
<i>R<sub>a</sub></i> = 5.262773292<br />
<i>K<sub>b</sub></i> = 0.4952900056<br />
<i>K</i><sub>τ</sub><i></i> = 0.3233728703<br />
<i>B</i> = 0.0006001689451<br />
<i>L<sub>a</sub></i> = 0.0047<br />
<i>J</i> = 0.001321184025<br />
<i>A<sub>r</sub></i> = 0.007299397206<br />
<br />
To find the inversion of the transfer function we should make some approximations. I assumed:<br />
<i>ω</i>(0) = <i>ω<sub>0</sub></i>, <i>i</i>(0) = f(<i>ω<sub>0</sub></i>).<br />
<br />
To calculate f(ω0) I supposed that the system is in steady state (ω=const. and i=const.) at the initial moment. In this case these differential equations are represented by the following linear equations.<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgB0h0aFqeYz47OlNDueXN070sz-sZdOgUw5AX57DMA-GBm62vyvVqrwPpTFzZcD7XNqtZxvJSFXe_VFiTSaqa4mMRfv8-SvZeEqsgHELzPLWOQ7p_78FEmaRnJGDMr2IkHHgfii8kkHiBK/s1600/equation2.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgB0h0aFqeYz47OlNDueXN070sz-sZdOgUw5AX57DMA-GBm62vyvVqrwPpTFzZcD7XNqtZxvJSFXe_VFiTSaqa4mMRfv8-SvZeEqsgHELzPLWOQ7p_78FEmaRnJGDMr2IkHHgfii8kkHiBK/s1600/equation2.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgB0h0aFqeYz47OlNDueXN070sz-sZdOgUw5AX57DMA-GBm62vyvVqrwPpTFzZcD7XNqtZxvJSFXe_VFiTSaqa4mMRfv8-SvZeEqsgHELzPLWOQ7p_78FEmaRnJGDMr2IkHHgfii8kkHiBK/s1600/equation2.PNG" /></a></div>
<br />
From this system of equations we can find <i>i</i>(0):<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh2lnPLiw02FbIkwKYrol4sI7jD-Vns6iJeLGs7_Mo-CEx4Jue8M006na3ZLbsBWp55i_8w_PgpPLC4PDzVK70m4XLUOKWO1jfU2qHPpmwcs7iB60rV7KpG5xK8g1jdT8hFcD56SNtXNuLR/s1600/equation3.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh2lnPLiw02FbIkwKYrol4sI7jD-Vns6iJeLGs7_Mo-CEx4Jue8M006na3ZLbsBWp55i_8w_PgpPLC4PDzVK70m4XLUOKWO1jfU2qHPpmwcs7iB60rV7KpG5xK8g1jdT8hFcD56SNtXNuLR/s1600/equation3.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh2lnPLiw02FbIkwKYrol4sI7jD-Vns6iJeLGs7_Mo-CEx4Jue8M006na3ZLbsBWp55i_8w_PgpPLC4PDzVK70m4XLUOKWO1jfU2qHPpmwcs7iB60rV7KpG5xK8g1jdT8hFcD56SNtXNuLR/s1600/equation3.PNG" /></a></div>
Solving the equations we can find the angular velocity function:<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjqQaG9zknAmfprRvlO31AnZekheRWOhI-peQ-6lu6SX5re36V7iVBSmDVe1fLQsRZfF82-npoh1I3acYon5eh6QciDYW5OD1MnIxc38WItiXNZ1GPy-k1Dkxek2nI1q5yujq76Zs3TlxoI/s1600/solution.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjqQaG9zknAmfprRvlO31AnZekheRWOhI-peQ-6lu6SX5re36V7iVBSmDVe1fLQsRZfF82-npoh1I3acYon5eh6QciDYW5OD1MnIxc38WItiXNZ1GPy-k1Dkxek2nI1q5yujq76Zs3TlxoI/s1600/solution.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjqQaG9zknAmfprRvlO31AnZekheRWOhI-peQ-6lu6SX5re36V7iVBSmDVe1fLQsRZfF82-npoh1I3acYon5eh6QciDYW5OD1MnIxc38WItiXNZ1GPy-k1Dkxek2nI1q5yujq76Zs3TlxoI/s1600/solution.PNG" /></a></div>
<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgEFaQm1ochrLuYR6dBNYLg6tA_uVb4tik4FGPgbcnpLlGyW0rxE3QDhWjyRM9rp59PcrixDZwCCXlV66zwX514XamYGIdZ1MYHXoEImrHaRWRlbEO-JJcSbveX2pBox5UBSxPhhNiv4mKc/s1600/solution.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"></a></div>
We want to find the inverse function in form <i> voltage</i>(<i>distance</i>, <i>ω<sub>0</sub></i>). To do this, we need to find integral of the angular velocity and then find the inverse function from it.<br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhWa0XGW38wRnaDZ27GXOeoQUOT3sTOT9NanK0NDxD-xKqxIrjMTqaqpoxgROkoT_xTI2LMZy49V3KndSIh7Df7aDvVMm03_-FVehxQDqDfotLys74N0lFzGPlwAYba1YablGuWbuqBpdnU/s1600/path-expression.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhWa0XGW38wRnaDZ27GXOeoQUOT3sTOT9NanK0NDxD-xKqxIrjMTqaqpoxgROkoT_xTI2LMZy49V3KndSIh7Df7aDvVMm03_-FVehxQDqDfotLys74N0lFzGPlwAYba1YablGuWbuqBpdnU/s1600/path-expression.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhWa0XGW38wRnaDZ27GXOeoQUOT3sTOT9NanK0NDxD-xKqxIrjMTqaqpoxgROkoT_xTI2LMZy49V3KndSIh7Df7aDvVMm03_-FVehxQDqDfotLys74N0lFzGPlwAYba1YablGuWbuqBpdnU/s1600/path-expression.PNG" /></a></div>
The common solution is too complex to show here. I found a set of specific solutions for particular discretization periods. For example, for discretization periods 4ms and 25ms the inverse functions are:<br />
<br />
power = (7299431.476·distance+11879.49780·signum(velocity)-28316.23421·velocity) / Battery.getVoltageMilliVolt()<br />
<br />
power = (152012.7242·distance+11879.49771·signum(velocity)-2918.826420·velocity) / Battery.getVoltageMilliVolt()<br />
<br />
To check accuracy of this function we can disable PID regulator and test the motor using only feed-forward component.<br />
<br />
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi97kfIQO70d8gENmqKCQ62Tet8-P9xEcypqQJ7UVu2jiv5SZFmps8MxpM0KfSt4jUIb3QqYjybdUkNEJdVzgnXl8XrgfuO9eKj7i6mYdn90KfrxaO8z7j3e6kEJy_l-aI2dBXvjcoqcera/s1600/rotate-400-ff.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="428" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi97kfIQO70d8gENmqKCQ62Tet8-P9xEcypqQJ7UVu2jiv5SZFmps8MxpM0KfSt4jUIb3QqYjybdUkNEJdVzgnXl8XrgfuO9eKj7i6mYdn90KfrxaO8z7j3e6kEJy_l-aI2dBXvjcoqcera/s640/rotate-400-ff.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi97kfIQO70d8gENmqKCQ62Tet8-P9xEcypqQJ7UVu2jiv5SZFmps8MxpM0KfSt4jUIb3QqYjybdUkNEJdVzgnXl8XrgfuO9eKj7i6mYdn90KfrxaO8z7j3e6kEJy_l-aI2dBXvjcoqcera/s640/rotate-400-ff.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Feed-forward only regulation on emulator.</td></tr>
</tbody></table>
<br />
<br />
At the end of rotation the error is 1degree. This result has been got on emulator. We can expect that the controller will work without errors on emulator, but rounding the motor power value produces some non-linearity.<br />
The following diagram shows the real motor measurement.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhXHtI84wYAUSDgPMbhP4IHGeMZxpvPEQORavamyPGbnTpLmidOknCFvLIKksdOqhfrkMhYBDfyyMRwx-bW0l6QM7OO0AViMkRAVq2tZF5aNQ_e1ru9pVN0UStgIYf53VU2VH26LLh51dX8/s1600/rotate-400-ff-real-initial.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="406" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhXHtI84wYAUSDgPMbhP4IHGeMZxpvPEQORavamyPGbnTpLmidOknCFvLIKksdOqhfrkMhYBDfyyMRwx-bW0l6QM7OO0AViMkRAVq2tZF5aNQ_e1ru9pVN0UStgIYf53VU2VH26LLh51dX8/s640/rotate-400-ff-real-initial.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhXHtI84wYAUSDgPMbhP4IHGeMZxpvPEQORavamyPGbnTpLmidOknCFvLIKksdOqhfrkMhYBDfyyMRwx-bW0l6QM7OO0AViMkRAVq2tZF5aNQ_e1ru9pVN0UStgIYf53VU2VH26LLh51dX8/s640/rotate-400-ff-real-initial.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Feed-forward only regulation on Lego motor.</td></tr>
</tbody></table>
We can see that position error is quite big (14 degrees at the end of rotation). Also we can see that the maximal motor's velocity is smaller than calculated velocity. I tuned <i>K<sub>b</sub></i> and<i> </i><i>A<sub>r</sub></i><i> </i>values and recalculated the feed-forward constants.<br />
<br />
<i>K<sub>b</sub></i><i>=</i>0.5002900056<br />
<i>A<sub>r</sub></i><i>=</i>0.009599397206 <br />
<br />
New feed-forward functions for 4ms and 25ms discretization periods are:<br />
<br />
power = (7300460.329·distance + 15622.66220·signum(velocity) - 28311.62297·velocity) / Battery.getVoltageMilliVolt()<br />
<br />
power = (152250.9950·distance + 15622.66225·signum(velocity) - 2916.056542·velocity) / Battery.getVoltageMilliVolt()<br />
<br />
In this case the result looks better:<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgQE60a_BCO5xxfgzvGQ9e0dG-KnztBmYEIeuQJeTPZO6U0tECEYL8OmvqjvsX03NTlfIFpnYTrb2Oh1F2DkKGjKPDotYDlsz33D_btxOLjK-YZQUgt_4XmZVGev-HjqMNYwbYMpfL9P5aA/s1600/rotate-400-ff-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="428" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgQE60a_BCO5xxfgzvGQ9e0dG-KnztBmYEIeuQJeTPZO6U0tECEYL8OmvqjvsX03NTlfIFpnYTrb2Oh1F2DkKGjKPDotYDlsz33D_btxOLjK-YZQUgt_4XmZVGev-HjqMNYwbYMpfL9P5aA/s640/rotate-400-ff-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgQE60a_BCO5xxfgzvGQ9e0dG-KnztBmYEIeuQJeTPZO6U0tECEYL8OmvqjvsX03NTlfIFpnYTrb2Oh1F2DkKGjKPDotYDlsz33D_btxOLjK-YZQUgt_4XmZVGev-HjqMNYwbYMpfL9P5aA/s640/rotate-400-ff-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Feed-forward only regulation on Lego motor. Rotation to 400 degrees.</td><td class="tr-caption" style="text-align: center;"><br /></td><td class="tr-caption" style="text-align: center;"><br /></td><td class="tr-caption" style="text-align: center;"><br /></td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjJtON1ZOjY15Cnho7WVp_i1D3xZjoUYUDeeWKGf_G2UJ9RD8puoYGt4inhRAd1DLwAab6VmowFyl4SK8QB7F9p6cCQ5jAr_RLdfPNQA8Ca50MPOhdc_EOLqnY6v3cmfPH7L189N2_e5qWL/s1600/rotate-4000-ff-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="386" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjJtON1ZOjY15Cnho7WVp_i1D3xZjoUYUDeeWKGf_G2UJ9RD8puoYGt4inhRAd1DLwAab6VmowFyl4SK8QB7F9p6cCQ5jAr_RLdfPNQA8Ca50MPOhdc_EOLqnY6v3cmfPH7L189N2_e5qWL/s640/rotate-4000-ff-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjJtON1ZOjY15Cnho7WVp_i1D3xZjoUYUDeeWKGf_G2UJ9RD8puoYGt4inhRAd1DLwAab6VmowFyl4SK8QB7F9p6cCQ5jAr_RLdfPNQA8Ca50MPOhdc_EOLqnY6v3cmfPH7L189N2_e5qWL/s640/rotate-4000-ff-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Feed-forward only regulation on Lego motor. Rotation to 4000 degrees.</td></tr>
</tbody></table>
The error on the first graph has maximum value 7.3 and is decreasing to 2 at the target point<br />
<br />
Then we will test the controller with closed feed-back loop. I used two scenarios - rotation to 400 and 4000 degrees. To estimate impact of feed-forward I will test the motor in two modes: feed-back only and feed-back with feed-forward. I used PI regulator in feed-back loop. The motor controller performs rotation to the specified angle and when this angle is reached and regulation error is less that 1 the controller switches to "stop" state where it keeps the current position. All following tests run until the motor switches to "stop" state.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjjqMDFwMKBgvQ2RFOhSllqiNWD9_276qaWxsEHjeVPVIjkIIFJcdt9cX-auPaUDA7ZmX99t9kidM7EZze1DpYUU8N8xq4zRY9ZVHcRPuFb4x6GUycJsZfJLEHz8dQfijgk1HBKQMjOGVwv/s1600/rotate-400-fb-calc.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="382" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjjqMDFwMKBgvQ2RFOhSllqiNWD9_276qaWxsEHjeVPVIjkIIFJcdt9cX-auPaUDA7ZmX99t9kidM7EZze1DpYUU8N8xq4zRY9ZVHcRPuFb4x6GUycJsZfJLEHz8dQfijgk1HBKQMjOGVwv/s640/rotate-400-fb-calc.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjjqMDFwMKBgvQ2RFOhSllqiNWD9_276qaWxsEHjeVPVIjkIIFJcdt9cX-auPaUDA7ZmX99t9kidM7EZze1DpYUU8N8xq4zRY9ZVHcRPuFb4x6GUycJsZfJLEHz8dQfijgk1HBKQMjOGVwv/s640/rotate-400-fb-calc.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 400 degrees using only feed-back. Emulator.</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhsUqPRHSdNuWqN2Kd90rtoDanjfNOUVsd3xNYnoDFmR8xz43Ux7N1IP9I0Cpx35t_Dh7S95MhhVhUgWAb0FpN6yXk9WdXi0NaNLCTVJO2XTMZW1BbRedzNe2sgXjb8lv4DQ6TFyk5_Vo_a/s1600/rotate-4000-fb-calc.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="394" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhsUqPRHSdNuWqN2Kd90rtoDanjfNOUVsd3xNYnoDFmR8xz43Ux7N1IP9I0Cpx35t_Dh7S95MhhVhUgWAb0FpN6yXk9WdXi0NaNLCTVJO2XTMZW1BbRedzNe2sgXjb8lv4DQ6TFyk5_Vo_a/s640/rotate-4000-fb-calc.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhsUqPRHSdNuWqN2Kd90rtoDanjfNOUVsd3xNYnoDFmR8xz43Ux7N1IP9I0Cpx35t_Dh7S95MhhVhUgWAb0FpN6yXk9WdXi0NaNLCTVJO2XTMZW1BbRedzNe2sgXjb8lv4DQ6TFyk5_Vo_a/s640/rotate-4000-fb-calc.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 4000 degrees using only feed-back. Emulator.</td></tr>
</tbody></table>
We can see that the motor does not reach the target position with the zero speed and performs some additional movements to reach it. Real motor shows the same behavior.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhdJwFQ8O9if6dO_ieMT1JzrmYHQDwA5cwfxjTDIF134MJyPfycrwJwaHwDHE22A4S-oxM_CLg9vWvq19V0b9tEcb0DOwvij_uHwkvFp4pdR15OyLrw1GTGeg0cFlbr5rW1SX4wVKP1GUwU/s1600/rotate-400-fb-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="400" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhdJwFQ8O9if6dO_ieMT1JzrmYHQDwA5cwfxjTDIF134MJyPfycrwJwaHwDHE22A4S-oxM_CLg9vWvq19V0b9tEcb0DOwvij_uHwkvFp4pdR15OyLrw1GTGeg0cFlbr5rW1SX4wVKP1GUwU/s640/rotate-400-fb-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhdJwFQ8O9if6dO_ieMT1JzrmYHQDwA5cwfxjTDIF134MJyPfycrwJwaHwDHE22A4S-oxM_CLg9vWvq19V0b9tEcb0DOwvij_uHwkvFp4pdR15OyLrw1GTGeg0cFlbr5rW1SX4wVKP1GUwU/s640/rotate-400-fb-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 400 degrees using only feed-back. NXT motor.</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgTw0_AHkNnPGQVPYiMPTkzeAqapBEW8zewBX7X6TekUELSHUvMletvjgWBhBccyK5PdJrKJ9UY6sx_kyBJtXZLwoKuUsRhaplrokMr3TX9e7f1dhlmi1QIHgh7Qw3locGDK7NQNR4jVdHr/s1600/rotate-4000-fb-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="374" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgTw0_AHkNnPGQVPYiMPTkzeAqapBEW8zewBX7X6TekUELSHUvMletvjgWBhBccyK5PdJrKJ9UY6sx_kyBJtXZLwoKuUsRhaplrokMr3TX9e7f1dhlmi1QIHgh7Qw3locGDK7NQNR4jVdHr/s640/rotate-4000-fb-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgTw0_AHkNnPGQVPYiMPTkzeAqapBEW8zewBX7X6TekUELSHUvMletvjgWBhBccyK5PdJrKJ9UY6sx_kyBJtXZLwoKuUsRhaplrokMr3TX9e7f1dhlmi1QIHgh7Qw3locGDK7NQNR4jVdHr/s640/rotate-4000-fb-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 4000 degrees using only feed-back. NXT motor.</td></tr>
</tbody></table>
We can see that results are very similar to results on emulator. Then we will test how feed-back and feed-forward are working together.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhwzftHbY3bhKbE8Avp3aJgcWy4T7mJVHYINqixowwMIbQ-Q-T18WMtfawhWrTmlm4fSNciLScHwX7z9Gf0PWzoOnX4mIOchnUWypFzkk7rPDkOO1g451CXlyVV0Rd4AKAeqUinJFWxtc_p/s1600/rotate-400-fb-ff-calc.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="382" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhwzftHbY3bhKbE8Avp3aJgcWy4T7mJVHYINqixowwMIbQ-Q-T18WMtfawhWrTmlm4fSNciLScHwX7z9Gf0PWzoOnX4mIOchnUWypFzkk7rPDkOO1g451CXlyVV0Rd4AKAeqUinJFWxtc_p/s640/rotate-400-fb-ff-calc.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhwzftHbY3bhKbE8Avp3aJgcWy4T7mJVHYINqixowwMIbQ-Q-T18WMtfawhWrTmlm4fSNciLScHwX7z9Gf0PWzoOnX4mIOchnUWypFzkk7rPDkOO1g451CXlyVV0Rd4AKAeqUinJFWxtc_p/s640/rotate-400-fb-ff-calc.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 400 degrees using feed-back and feed-forward. Emulator.</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgbVBJOaR8ctvjJMkGW__xN3meEJ5qpV_PRr9scBydCezggfnzcM8SmHX0DHMUVSrTDyG-PwGHOzgrm8e1sV4PKyN6lM1WS2O634QxBlIF3oCjitg_ZdrGtF41UduiSAc_Hu5WZPPd_uF8b/s1600/rotate-4000-fb-ff-calc.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="404" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgbVBJOaR8ctvjJMkGW__xN3meEJ5qpV_PRr9scBydCezggfnzcM8SmHX0DHMUVSrTDyG-PwGHOzgrm8e1sV4PKyN6lM1WS2O634QxBlIF3oCjitg_ZdrGtF41UduiSAc_Hu5WZPPd_uF8b/s640/rotate-4000-fb-ff-calc.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgbVBJOaR8ctvjJMkGW__xN3meEJ5qpV_PRr9scBydCezggfnzcM8SmHX0DHMUVSrTDyG-PwGHOzgrm8e1sV4PKyN6lM1WS2O634QxBlIF3oCjitg_ZdrGtF41UduiSAc_Hu5WZPPd_uF8b/s640/rotate-4000-fb-ff-calc.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 4000 degrees using feed-back and feed-forward. Emulator.</td></tr>
</tbody></table>
We can see that combining feed-back and feed-forward reduces regulation error and the motor reaches the target point with zero speed.<br />
Then we will test the controller using the real motor.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgMm5H1XyUEDCs7w5zRjFcjehHlYbRkzhUL8K8vZ6jfG2O6Tnq5NxABYTYfI1APv4yTq8oIOidLXBmCYr7p_W1e1CGnhGdUR4-jkmAtYpgX3KvL-l7MXPMkHdLfENmzmR-aKvDfIcnWT5Lm/s1600/rotate-400-fb-ff-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="442" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgMm5H1XyUEDCs7w5zRjFcjehHlYbRkzhUL8K8vZ6jfG2O6Tnq5NxABYTYfI1APv4yTq8oIOidLXBmCYr7p_W1e1CGnhGdUR4-jkmAtYpgX3KvL-l7MXPMkHdLfENmzmR-aKvDfIcnWT5Lm/s640/rotate-400-fb-ff-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgMm5H1XyUEDCs7w5zRjFcjehHlYbRkzhUL8K8vZ6jfG2O6Tnq5NxABYTYfI1APv4yTq8oIOidLXBmCYr7p_W1e1CGnhGdUR4-jkmAtYpgX3KvL-l7MXPMkHdLfENmzmR-aKvDfIcnWT5Lm/s640/rotate-400-fb-ff-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 400 degrees using feed-back and feed-forward. NXT motor.</td></tr>
</tbody></table>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjAgfaNw8pSh6eD0psLxgYWGV2ttJIzAU9oJbCcAE3hKJNEgzL_4jonetz410DXXB3CdKHfD_GdWF66RZkLq1aSFbwATdaTbwLpp_oKBZOqPrStsJR6XuecH1WMF_3QrXjb2Bp83i5-DHM5/s1600/rotate-4000-fb-ff-real.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="416" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjAgfaNw8pSh6eD0psLxgYWGV2ttJIzAU9oJbCcAE3hKJNEgzL_4jonetz410DXXB3CdKHfD_GdWF66RZkLq1aSFbwATdaTbwLpp_oKBZOqPrStsJR6XuecH1WMF_3QrXjb2Bp83i5-DHM5/s640/rotate-4000-fb-ff-real.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjAgfaNw8pSh6eD0psLxgYWGV2ttJIzAU9oJbCcAE3hKJNEgzL_4jonetz410DXXB3CdKHfD_GdWF66RZkLq1aSFbwATdaTbwLpp_oKBZOqPrStsJR6XuecH1WMF_3QrXjb2Bp83i5-DHM5/s640/rotate-4000-fb-ff-real.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Rotation to 4000 degrees using feed-back and feed-forward. NXT motor.</td></tr>
</tbody></table>
We can see that real motor behavior is very similar to emulation. And the motor also reaches the target point with zero speed.<br />
<br />
<b><span style="font-size: large;">Feed-back regulator</span></b><br />
As feed-back regulator I used PID regulator. I used algorithm described in [1]. I turned off derivative part and used the controller as PI controller, because derivative component produces high-frequency noise and makes rotation precision worse. But PI controller has some disadvantages.<br />
Let's compare how LeJOS motor controller and my controller respond to change of load force. I will use the emulator to test scenario when the motor runs without load, but from 4 till 5 second some external load is applied to the motor's shaft.<br />
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiuZdmOW8EKLCh8TCOwK2xXj0FWmQ2XbL1P1eSg5h_cC6y2zdsvWwTivuHEXwecVI1D6KTB5sMMd0fUMYXlSHHVGKiuF7z-OATizTQGjR45rOsZ9gTjZFEtDdwQW19lrHNuORYip1nJ4rNF/s1600/step-responce-my.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="392" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiuZdmOW8EKLCh8TCOwK2xXj0FWmQ2XbL1P1eSg5h_cC6y2zdsvWwTivuHEXwecVI1D6KTB5sMMd0fUMYXlSHHVGKiuF7z-OATizTQGjR45rOsZ9gTjZFEtDdwQW19lrHNuORYip1nJ4rNF/s640/step-responce-my.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiuZdmOW8EKLCh8TCOwK2xXj0FWmQ2XbL1P1eSg5h_cC6y2zdsvWwTivuHEXwecVI1D6KTB5sMMd0fUMYXlSHHVGKiuF7z-OATizTQGjR45rOsZ9gTjZFEtDdwQW19lrHNuORYip1nJ4rNF/s640/step-responce-my.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Step response of my regulator</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEglLjId5_ozzvXPuFCtTozrOdnniNEny-bIBKUs3uzjl29JFrwW0BrldzrQWtTjqOiLMWfAdiiMcuHQpA0pA8uCfR_dw5KuLWqu5ww-XdP1HtWqY8YZtwEQEm7EiJtHh-NGysRRnsqFcxuG/s1600/step-responce-lejos.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" height="352" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEglLjId5_ozzvXPuFCtTozrOdnniNEny-bIBKUs3uzjl29JFrwW0BrldzrQWtTjqOiLMWfAdiiMcuHQpA0pA8uCfR_dw5KuLWqu5ww-XdP1HtWqY8YZtwEQEm7EiJtHh-NGysRRnsqFcxuG/s640/step-responce-lejos.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEglLjId5_ozzvXPuFCtTozrOdnniNEny-bIBKUs3uzjl29JFrwW0BrldzrQWtTjqOiLMWfAdiiMcuHQpA0pA8uCfR_dw5KuLWqu5ww-XdP1HtWqY8YZtwEQEm7EiJtHh-NGysRRnsqFcxuG/s640/step-responce-lejos.PNG" width="640" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Step response of LeJOS regulator</td></tr>
</tbody></table>
We can see that LeJOS controller works more effective and its overshot is much lesser. But LeJOS uses 4ms discretization interval, it allows to increase proportional gain and derivative time and decrease integration time to improve response time of controller. I wanted to decrease CPU load, therefore I used discretization period 25ms. In this case I cannot use such parameter values because the controller becomes unstable.<br />
<br />
<b><span style="font-size: large;">CPU load</span></b><br />
I used two tests to measure CPU load. In the first test I executed the regulation function in loop, on fake data, and measured average execution time. In the second test I tried to measure CPU load on working motor. This test increments an integer variable during 5 seconds. The first run is performed without running motors. Its results serves as a base. The second run is performed when one motor is running. Ration of these results gives me a percent of available CPU time.<br />
<br />
Results of the first test:<br />
My controller: 754 microseconds.<br />
LeJOS controller: 595 microseconds.<br />
<br />
Execution time of my controller is bigger because it uses multiple classes to implement its parts: motion profile generator, feed-forward calculator and PID regulator. When I combined all of them in a single class, I found that this variant of controller requires 597 microseconds to run.<br />
<br />
The second test results (numbers of iterations):<br />
My controlller: base: 374955, motor is running: 366222. CPU load 2.33%<br />
LeJOS controller: base 374993, motor is running: 319285. CPU load 15%<br />
<br />
<br />
These results are for one motor. When other motors are running we should multiple these values to number of working motors.<br />
<br />
<b><span style="font-size: large;">Conclusion </span></b><br />
I wanted to make a motor controller that would provide the same interface as LeJOS motor but would consume less CPU time. I found that using long discretization periods together with feed-forward allow me to make such controller.<br />
Another reason for starting of this project was the fact that LeJOS motor had not worked well with low acceleration values. However, the current version of LeJOS controller works well, but requires much CPU time.<br />
<br />
By the way, I found another way to reduce CPU load in the motor controller. I converted my controller to C++, compiled it into native ARM code and tested it under nxtOSEK. In this case it requires only 61 microseconds to one regulation cycle. I think it is a interesting point to my next project.<br />
<br />
<b><span style="font-size: large;"></span></b><br />
<br />
<b><span style="font-size: large;">Bibliography</span></b><br />
<br />
[1] Åström, K. J. and T. Hägglund. Advanced PID control. - ISA - The Instrumentation, System, and<br />
Automation Society, 2006</div>
Maximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.com12tag:blogger.com,1999:blog-729812208796274999.post-37934210127859916402010-11-27T16:12:00.008+03:002012-10-05T17:22:52.616+04:00Compare motor controllers of LeJOS and Lego firmware<div dir="ltr" style="text-align: left;" trbidi="on">
In this post I'm going to compare two motor controllers: from Lego Mindstorms firmware and from <a href="http://lejos.sourceforge.net/">LeJOS</a> 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.<br />
<br />
<a name='more'></a><br />
I ported Lego PID-controller to Java, made the motor's emulator using motor characteristics measured by <a href="http://www.philohome.com/motors/motorcomp.htm">Philo</a>. Then I prepared a set of tests for different use-cases. I tested LeJOS development snapshot, revision 4037.<br />
<br />
<span style="font-size: large;"><b>No load run</b></span><br />
In this case we test speed up curve, speed stability and changing rotation direction. The motor rotates backwards for 5 seconds with speed of 600 degree/second, then starts to rotate forwards.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZACtimkw2ntN2Ma-6uAXJePbnjT8rsBE2rcZkTz-xJsPRL0wq_V-Bhep6T1mXvVheCu-XxKXvjecdrhcBVuFbhcZzdC1Q-b3MkD8DaP4YZkKWMkPZ7H5klMhuoz7vCIQvPBbftcqmS3OS/s1600/no_load1.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZACtimkw2ntN2Ma-6uAXJePbnjT8rsBE2rcZkTz-xJsPRL0wq_V-Bhep6T1mXvVheCu-XxKXvjecdrhcBVuFbhcZzdC1Q-b3MkD8DaP4YZkKWMkPZ7H5klMhuoz7vCIQvPBbftcqmS3OS/s1600/no_load1.PNG" style="margin-left: auto; margin-right: auto;" /></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running without external load</td><td class="tr-caption" style="text-align: center;"><br /></td></tr>
</tbody></table>
We can see that LeJOS controller has bigger overshot on changing direction but reaches the target speed more quickly.<br />
<br />
<b><span style="font-size: large;">Running with </span><span style="font-size: large;">external load 0.1 N*m</span></b><br />
In this case we apply constant external load to the motor. This load allows the motor to rotate with the specified speed 600 deg/sec. The load is applied in one direction.<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg6iGFCtIx9n_dpAQib6zH-UUQAIzwzUNonQg8_FqYkv6kjqEouL5S7WlORZbD3_-iEIpfijAiudxT1gdAbLZiNWqgtZWcwl_YWnreRwke2KMCyRITXEp8j2s3A4qAkcaHQAh-jPvZB1bsa/s1600/load-0.1.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg6iGFCtIx9n_dpAQib6zH-UUQAIzwzUNonQg8_FqYkv6kjqEouL5S7WlORZbD3_-iEIpfijAiudxT1gdAbLZiNWqgtZWcwl_YWnreRwke2KMCyRITXEp8j2s3A4qAkcaHQAh-jPvZB1bsa/s1600/load-0.1.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEg6iGFCtIx9n_dpAQib6zH-UUQAIzwzUNonQg8_FqYkv6kjqEouL5S7WlORZbD3_-iEIpfijAiudxT1gdAbLZiNWqgtZWcwl_YWnreRwke2KMCyRITXEp8j2s3A4qAkcaHQAh-jPvZB1bsa/s1600/load-0.1.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Run with external load 0.1 N*m</td></tr>
</tbody></table>
<br />
Both controllers work well.<br />
<br />
<br />
<span style="font-size: large;"><b>Running with external load 0.3 N*m</b></span><br />
In this case we apply high load that does not allow the motor rotating with the specified speed 600 deg/sec.<br />
And controllers' behavior is different. The load is applied in one direction as in the previous test.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZBNzaUJnRQLnoJdiMCmC4IksJwvc1YSwpQOgbZKhHwsh8CgGTJ4P50UUl_jyd7VkX2ymCKg6FcIXD_XX2MFpk-2lr0CPY5Fv_acrz5di5SUhcY2PolECscktQdp0duQF1oq8DucIiO-uf/s1600/load-0.3.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZBNzaUJnRQLnoJdiMCmC4IksJwvc1YSwpQOgbZKhHwsh8CgGTJ4P50UUl_jyd7VkX2ymCKg6FcIXD_XX2MFpk-2lr0CPY5Fv_acrz5di5SUhcY2PolECscktQdp0duQF1oq8DucIiO-uf/s1600/load-0.3.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhZBNzaUJnRQLnoJdiMCmC4IksJwvc1YSwpQOgbZKhHwsh8CgGTJ4P50UUl_jyd7VkX2ymCKg6FcIXD_XX2MFpk-2lr0CPY5Fv_acrz5di5SUhcY2PolECscktQdp0duQF1oq8DucIiO-uf/s1600/load-0.3.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with external load 0.3 N*m</td></tr>
</tbody></table>
We can see that LeJOS motor does not powered at the first moment after start, but after some period of time starts working, but after changing direction again loss the power. Then after some period it can start working, but after one second of running it stops finally. LeJOS controller has so called "stall detector" that stops the motor if the regulation error is too big. Unfortunately, we cannot turn off this detector using LeJOS motor controller interface. I turned it off in he source code and got the following diagram.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjq4IE5smI8E78GuG8uO_XDjX96nwo3o_FYjmQoe08UUGGCr5b28yw0swUlj3o5mvk2X619RUR1_0aSUOjeC8rSJCirjot1bBki9Qvn-zCEvYOsbxr6GTbhXbaZvZPEElWKpdSKRa21wIqg/s1600/load-0.3-no_stall_detect.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjq4IE5smI8E78GuG8uO_XDjX96nwo3o_FYjmQoe08UUGGCr5b28yw0swUlj3o5mvk2X619RUR1_0aSUOjeC8rSJCirjot1bBki9Qvn-zCEvYOsbxr6GTbhXbaZvZPEElWKpdSKRa21wIqg/s1600/load-0.3-no_stall_detect.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjq4IE5smI8E78GuG8uO_XDjX96nwo3o_FYjmQoe08UUGGCr5b28yw0swUlj3o5mvk2X619RUR1_0aSUOjeC8rSJCirjot1bBki9Qvn-zCEvYOsbxr6GTbhXbaZvZPEElWKpdSKRa21wIqg/s1600/load-0.3-no_stall_detect.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with external load 0.3 N*m without stall detection</td></tr>
</tbody></table>
<br />
We can see that LeJOS controller still works. Lego controller has big overshot at the first moment but switches direction without delay, which LeJOS controller has.<br />
<br />
<br />
<b><span style="font-size: large;">Variable load</span></b><br />
Now we can look into how the controllers can stabilize the rotation speed under variable external load. I use sine-modulated load from 0.05 to 0.15 N*m with different frequency. In this case the external load is bidirectional like the dry friction force.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhYvVmKLiB3Amc9Dro6uCRllptlDNnpDA7UNAjBl6Dm63qjbA3fBgxNogyG-cJAyoSnCtZX4n9UYfFYiaiT4xpgFqqc8muqbI-p2FTptOFvw8gD4w-pGbokeBU8GV62TC7aOxypkZXEoTvZ/s1600/load_0.1_variable-1.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhYvVmKLiB3Amc9Dro6uCRllptlDNnpDA7UNAjBl6Dm63qjbA3fBgxNogyG-cJAyoSnCtZX4n9UYfFYiaiT4xpgFqqc8muqbI-p2FTptOFvw8gD4w-pGbokeBU8GV62TC7aOxypkZXEoTvZ/s1600/load_0.1_variable-1.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhYvVmKLiB3Amc9Dro6uCRllptlDNnpDA7UNAjBl6Dm63qjbA3fBgxNogyG-cJAyoSnCtZX4n9UYfFYiaiT4xpgFqqc8muqbI-p2FTptOFvw8gD4w-pGbokeBU8GV62TC7aOxypkZXEoTvZ/s1600/load_0.1_variable-1.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with variable external load of low frequency</td><td class="tr-caption" style="text-align: center;"><br /></td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi4nmC4Gh5k6PR-CVd4-kv812jxqAOXh70MB66rX7IC_ivTchJyoYdmq5DKmOJ2ZueWfky8ZJddwkHM7m1GSZkAtjudbeHMoFqdN9pWiW_4KOX9bGEO4r-fg81JB0OkbzZBnXxsfzyw28BD/s1600/load_0.1_variable-1-low_speed.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi4nmC4Gh5k6PR-CVd4-kv812jxqAOXh70MB66rX7IC_ivTchJyoYdmq5DKmOJ2ZueWfky8ZJddwkHM7m1GSZkAtjudbeHMoFqdN9pWiW_4KOX9bGEO4r-fg81JB0OkbzZBnXxsfzyw28BD/s1600/load_0.1_variable-1-low_speed.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEi4nmC4Gh5k6PR-CVd4-kv812jxqAOXh70MB66rX7IC_ivTchJyoYdmq5DKmOJ2ZueWfky8ZJddwkHM7m1GSZkAtjudbeHMoFqdN9pWiW_4KOX9bGEO4r-fg81JB0OkbzZBnXxsfzyw28BD/s1600/load_0.1_variable-1-low_speed.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with variable external load of low frequency and low speed</td></tr>
</tbody></table>
We can see that both controllers can keep the specified speed under low-frequency modulated external load. LeJOS controller works better.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgnSpxHmWnGmalCtoArbur-K8GsvX6nj83lXqmTw72wRCjo0KYSVouHk4Z5-GV-ESlMkzkWLLQYT5ojVzS6oTcc7aI90kRxhRV3Ia_g08ltJvfs8PNU26df_8QNt7jWkopUNDi1YutJPw1E/s1600/load_0.1_variable-5.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgnSpxHmWnGmalCtoArbur-K8GsvX6nj83lXqmTw72wRCjo0KYSVouHk4Z5-GV-ESlMkzkWLLQYT5ojVzS6oTcc7aI90kRxhRV3Ia_g08ltJvfs8PNU26df_8QNt7jWkopUNDi1YutJPw1E/s1600/load_0.1_variable-5.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgnSpxHmWnGmalCtoArbur-K8GsvX6nj83lXqmTw72wRCjo0KYSVouHk4Z5-GV-ESlMkzkWLLQYT5ojVzS6oTcc7aI90kRxhRV3Ia_g08ltJvfs8PNU26df_8QNt7jWkopUNDi1YutJPw1E/s1600/load_0.1_variable-5.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with variable external load of medium frequency</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhqFQPMGPt2n-PK4u8Z7BrpiIEk3Z5INfNwoXpa8k6-epnpv9seHQSPiPdOVnW4VCm_kTlCQugdqQeq8pWeIrIhfGPaGY8D93q4-4CL5B5ksz4aYdvJ3gjpB9nGECnMIQXWMokCQkgbd2rn/s1600/load_0.1_variable-5-low_speed.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhqFQPMGPt2n-PK4u8Z7BrpiIEk3Z5INfNwoXpa8k6-epnpv9seHQSPiPdOVnW4VCm_kTlCQugdqQeq8pWeIrIhfGPaGY8D93q4-4CL5B5ksz4aYdvJ3gjpB9nGECnMIQXWMokCQkgbd2rn/s1600/load_0.1_variable-5-low_speed.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEhqFQPMGPt2n-PK4u8Z7BrpiIEk3Z5INfNwoXpa8k6-epnpv9seHQSPiPdOVnW4VCm_kTlCQugdqQeq8pWeIrIhfGPaGY8D93q4-4CL5B5ksz4aYdvJ3gjpB9nGECnMIQXWMokCQkgbd2rn/s1600/load_0.1_variable-5-low_speed.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Running with variable external load of medium frequency and low speed</td></tr>
</tbody></table>
We can see that in this case Lego controller works much worse than LeJOS one.<br />
<br />
<b><span style="font-size: large;">Holding the position against external load</span></b><br />
<br />
In this chapter we will test locking the motor axle against external load. Lego and LeJOS controllers use different approaches to keep axle position. Lego controller keeps the rotation speed to zero, but LeJOS controller uses some kind of PWM controller to keep the current position. Therefore Lego controller does not keep the current position so strictly as LeJOS one, and the offset depends on the load value. In this test we apply the load force in one direction.<br />
<br />
<table cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjP3Mfw-eLzZuuFwgdm1ssiwFNFgMhCOOAcW75U6Oht5dwzuTjerffW7JSe-oS2iT9ELL_90HUa8EeXsZ9ti_Na7tBUUaHD0aTDmwOd-_ha9jrPMr6MHYOWXBIEdLreKdkIJfNRGzHdkCGQ/s1600/lock-lego.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjP3Mfw-eLzZuuFwgdm1ssiwFNFgMhCOOAcW75U6Oht5dwzuTjerffW7JSe-oS2iT9ELL_90HUa8EeXsZ9ti_Na7tBUUaHD0aTDmwOd-_ha9jrPMr6MHYOWXBIEdLreKdkIJfNRGzHdkCGQ/s1600/lock-lego.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjP3Mfw-eLzZuuFwgdm1ssiwFNFgMhCOOAcW75U6Oht5dwzuTjerffW7JSe-oS2iT9ELL_90HUa8EeXsZ9ti_Na7tBUUaHD0aTDmwOd-_ha9jrPMr6MHYOWXBIEdLreKdkIJfNRGzHdkCGQ/s1600/lock-lego.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Lego FW locking diagram</td><td class="tr-caption" style="text-align: center;"><br /></td></tr>
</tbody></table>
<br />
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh-6yDlKVbrINYWTNAqVfOim7xFEJqdlZBmvEuExCz-bNmDR1EJnnOo5AJdAkPdk5ybf5PHE9-qL6-VlyK9UUYUXnXgMHhmipxG3imxDkI9sY1MzOUcyrvufFArmNGm3V6Yu_xIWApmkQUB/s1600/lock-lejos.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh-6yDlKVbrINYWTNAqVfOim7xFEJqdlZBmvEuExCz-bNmDR1EJnnOo5AJdAkPdk5ybf5PHE9-qL6-VlyK9UUYUXnXgMHhmipxG3imxDkI9sY1MzOUcyrvufFArmNGm3V6Yu_xIWApmkQUB/s1600/lock-lejos.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEh-6yDlKVbrINYWTNAqVfOim7xFEJqdlZBmvEuExCz-bNmDR1EJnnOo5AJdAkPdk5ybf5PHE9-qL6-VlyK9UUYUXnXgMHhmipxG3imxDkI9sY1MzOUcyrvufFArmNGm3V6Yu_xIWApmkQUB/s1600/lock-lejos.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LeJOS locking diagram</td></tr>
</tbody></table>
LeJOS controller has a bug in lock method implementation that prevent us to use custom locking power. It always use locking power 30 percent.<br />
<br />
I also tested locking mechanism against variable external load.<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEirOfxIfloaV7vi5LopL_ta8RmjfkablGdJjZtNWTqKBdAoEkR0vRigUQYbUin9as0s6MMX1voFYnBJRGlvVKzp2m4v9CafnZhTWrpEjnXj_DLvHGScA5vv47sBwIwGg4272RT-fuzku95y/s1600/lock_variable-lego.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEirOfxIfloaV7vi5LopL_ta8RmjfkablGdJjZtNWTqKBdAoEkR0vRigUQYbUin9as0s6MMX1voFYnBJRGlvVKzp2m4v9CafnZhTWrpEjnXj_DLvHGScA5vv47sBwIwGg4272RT-fuzku95y/s1600/lock_variable-lego.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEirOfxIfloaV7vi5LopL_ta8RmjfkablGdJjZtNWTqKBdAoEkR0vRigUQYbUin9as0s6MMX1voFYnBJRGlvVKzp2m4v9CafnZhTWrpEjnXj_DLvHGScA5vv47sBwIwGg4272RT-fuzku95y/s1600/lock_variable-lego.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Lego locking diagram. Variable external load.</td></tr>
</tbody></table>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgrqA5q76qRrsgMFGwds0wFsgmqFoPlD3ZXi7WnqMQWk89UFenak8loHPDG3FBIfhv92urzFpyVy_sm1GgHLOQdGHozla__iqe-tt1cjtzUBg2VoSR2W7uZK5U7F55AQPqROuW-mLe91cJ8/s1600/lock_variable-lejos.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgrqA5q76qRrsgMFGwds0wFsgmqFoPlD3ZXi7WnqMQWk89UFenak8loHPDG3FBIfhv92urzFpyVy_sm1GgHLOQdGHozla__iqe-tt1cjtzUBg2VoSR2W7uZK5U7F55AQPqROuW-mLe91cJ8/s1600/lock_variable-lejos.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgrqA5q76qRrsgMFGwds0wFsgmqFoPlD3ZXi7WnqMQWk89UFenak8loHPDG3FBIfhv92urzFpyVy_sm1GgHLOQdGHozla__iqe-tt1cjtzUBg2VoSR2W7uZK5U7F55AQPqROuW-mLe91cJ8/s1600/lock_variable-lejos.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LeJOS locking diagram. Variable external load</td></tr>
</tbody></table>
<div class="separator" style="clear: both; text-align: center;">
</div>
It seems that Lego approach works better because position oscillations are smaller.<br />
<br />
<b><span style="font-size: large;">Smooth acceleration</span></b><br />
Smooth acceleration one of features of LeJOS motor controller. Lego firmware have similar feature that allow to control acceleration an deceleration explicitly. But I found this approach not convenient and implemented smooth acceleration in my Java version of Lego controller.<br />
I found that LeJOS implementation have a serious bug that makes this feature much less useful that it could be.<br />
I use two scenarios for testing this feature:<br />
<b>The first scenario<span style="font-size: small;">:</span></b> <span style="font-family: "Courier New",Courier,monospace;"><span style="font-size: small;"><span style="font-family: Arial,Helvetica,sans-serif;"></span></span></span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.smoothAcceleration(true);</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.setAcceleration(300);</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.setSpeed(300);</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.forward();</span><br />
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<div style="font-family: "Courier New",Courier,monospace;">
</div>
<div style="font-family: "Courier New",Courier,monospace;">
motor.setSpeed(600);</div>
<div style="font-family: "Courier New",Courier,monospace;">
</div>
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<span style="font-family: "Courier New",Courier,monospace;"> motor.setSpeed(100);</span><br />
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjXCxEjT-QJRi0PfX2Q69JZ-FnkbbjIYLVfG5bJJhzijRj4JCVSm3nSKEnprxobTmBXtZkmCdTSBxw_amh8v3m_03UDSUHRfsCY238HkzKmcZZzJN_4UQ0Rp5JT038HZpUM6EhLQvwuVLEU/s1600/smooth_acceleration-lego.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjXCxEjT-QJRi0PfX2Q69JZ-FnkbbjIYLVfG5bJJhzijRj4JCVSm3nSKEnprxobTmBXtZkmCdTSBxw_amh8v3m_03UDSUHRfsCY238HkzKmcZZzJN_4UQ0Rp5JT038HZpUM6EhLQvwuVLEU/s1600/smooth_acceleration-lego.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjXCxEjT-QJRi0PfX2Q69JZ-FnkbbjIYLVfG5bJJhzijRj4JCVSm3nSKEnprxobTmBXtZkmCdTSBxw_amh8v3m_03UDSUHRfsCY238HkzKmcZZzJN_4UQ0Rp5JT038HZpUM6EhLQvwuVLEU/s1600/smooth_acceleration-lego.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Lego implementation of smooth acceleration</td></tr>
</tbody></table>
<br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEibl4XVZ8-dIXFopBiSrQIJJcDtu55GW-zpOVjL7-N0nj4TlLBHxO23BaUs98hL_xwO3U6h-NkRPbBqD2fBQD3pKmjmOduGCYtpHyho8_AUYjexuV-L_IJ8hzeb_JHAKuOckR3CFm4DQn5z/s1600/smooth_acceleration-lejos.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEibl4XVZ8-dIXFopBiSrQIJJcDtu55GW-zpOVjL7-N0nj4TlLBHxO23BaUs98hL_xwO3U6h-NkRPbBqD2fBQD3pKmjmOduGCYtpHyho8_AUYjexuV-L_IJ8hzeb_JHAKuOckR3CFm4DQn5z/s1600/smooth_acceleration-lejos.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEibl4XVZ8-dIXFopBiSrQIJJcDtu55GW-zpOVjL7-N0nj4TlLBHxO23BaUs98hL_xwO3U6h-NkRPbBqD2fBQD3pKmjmOduGCYtpHyho8_AUYjexuV-L_IJ8hzeb_JHAKuOckR3CFm4DQn5z/s1600/smooth_acceleration-lejos.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LeJOS implementation of smooth acceleration</td></tr>
</tbody></table>
LeJOS diagram shows that LeJOS controller resets the smooth acceleration data at speed change. <br />
<br />
<b>The second scenario:</b><br />
<br />
<div style="font-family: "Courier New",Courier,monospace;">
motor.smoothAcceleration(true);<br />
motor.setAcceleration(50);<br />
motor.setSpeed(600);<br />
motor.forward();</div>
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<div style="font-family: "Courier New",Courier,monospace;">
motor.setAcceleration(300);</div>
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<div style="font-family: "Courier New",Courier,monospace;">
motor.setSpeed(100);</div>
<div style="font-family: "Courier New",Courier,monospace;">
<wait></div>
<span style="font-family: "Courier New",Courier,monospace;"> motor.setSpeed(600);</span><br />
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEizR5bQJNOA6nGZ59djSa_OMtZqOjd_yU14RLa2K1u-YJZD2lykTebjolx1Ha8_3bfFcRggttAXxmzpYpqZ6ELp7sgIzm9FjutjF7RWZUeMQP5iLFVAtlT_h6IFPu5kaLxfsoMHwIWCT3a5/s1600/variable_smooth_acceleration-lego.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEizR5bQJNOA6nGZ59djSa_OMtZqOjd_yU14RLa2K1u-YJZD2lykTebjolx1Ha8_3bfFcRggttAXxmzpYpqZ6ELp7sgIzm9FjutjF7RWZUeMQP5iLFVAtlT_h6IFPu5kaLxfsoMHwIWCT3a5/s1600/variable_smooth_acceleration-lego.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEizR5bQJNOA6nGZ59djSa_OMtZqOjd_yU14RLa2K1u-YJZD2lykTebjolx1Ha8_3bfFcRggttAXxmzpYpqZ6ELp7sgIzm9FjutjF7RWZUeMQP5iLFVAtlT_h6IFPu5kaLxfsoMHwIWCT3a5/s1600/variable_smooth_acceleration-lego.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">Lego implementation of smooth acceleration</td></tr>
</tbody></table>
<table align="center" cellpadding="0" cellspacing="0" class="tr-caption-container" style="margin-left: auto; margin-right: auto; text-align: center;"><tbody>
<tr><td style="text-align: center;"><a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj9-LqEd4zzdNNdoPyJno7uohCr1MPlXHADdYWhu_vNI80kMy2WFd8VuWNJvDD5Z3ZxqeDGPv-yb2qwLDEWE096W6QyUYw84FGJBZDLee-aBf-QJ1G-NZlKxNrGwdLO69_DwY67FLi8NT17/s1600/variable_smooth_acceleration-lejos.PNG" imageanchor="1" style="margin-left: auto; margin-right: auto;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj9-LqEd4zzdNNdoPyJno7uohCr1MPlXHADdYWhu_vNI80kMy2WFd8VuWNJvDD5Z3ZxqeDGPv-yb2qwLDEWE096W6QyUYw84FGJBZDLee-aBf-QJ1G-NZlKxNrGwdLO69_DwY67FLi8NT17/s1600/variable_smooth_acceleration-lejos.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj9-LqEd4zzdNNdoPyJno7uohCr1MPlXHADdYWhu_vNI80kMy2WFd8VuWNJvDD5Z3ZxqeDGPv-yb2qwLDEWE096W6QyUYw84FGJBZDLee-aBf-QJ1G-NZlKxNrGwdLO69_DwY67FLi8NT17/s1600/variable_smooth_acceleration-lejos.PNG" /></a></td></tr>
<tr><td class="tr-caption" style="text-align: center;">LeJOS implementation of smooth acceleration</td></tr>
</tbody></table>
We see the same behavior at acceleration change. It seems LeJOS controller resets its state when speed or acceleration is changed. Also we can see that LeJOS interprets the specifies acceleration value incorrectly, because LeJOS controller's acceleration curve has less inclination than in my Lego-based implementation. Ff we set acceleration value of 300 degree/sec/sec we expect that after one second period of time the rotation speed will be 300 degree/sec, but LeJOS controller does not provide such behavior.<br />
<br />
<span style="font-size: small;"><b>The third scenario:</b></span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.smoothAcceleration(true);</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.setAcceleration(300);</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.setSpeed(speed);</span><br />
<br />
<span style="font-family: "Courier New",Courier,monospace;"> motor.backward();</span><br />
<span style="font-family: "Courier New",Courier,monospace;"></span><span style="font-family: "Courier New",Courier,monospace;"> <wait></span><br />
<span style="font-family: "Courier New",Courier,monospace;"></span><span style="font-family: "Courier New",Courier,monospace;"> motor.forward();</span><br />
<span style="font-family: "Courier New",Courier,monospace;"> <wait></span><br />
<span style="font-family: "Courier New",Courier,monospace;"></span><span style="font-family: "Courier New",Courier,monospace;"></span><br />
<div class="separator" style="clear: both; text-align: center;">
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgXJDtFpkmxGB4XxOnseT1HYzcH12Pham642x1R0bltj5E5L3iCAYPK8kJvoybtmO3gSAAHHxcww-V3u68OL3aTWG6vWMtI2EnTsZQ432Gz_ov3frjHkWcOq8B718dzZ34i85PeZzGaa0UD/s1600/change-direction-smooth.PNG" imageanchor="1" style="margin-left: 1em; margin-right: 1em;"><img border="0" ilo-full-src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgXJDtFpkmxGB4XxOnseT1HYzcH12Pham642x1R0bltj5E5L3iCAYPK8kJvoybtmO3gSAAHHxcww-V3u68OL3aTWG6vWMtI2EnTsZQ432Gz_ov3frjHkWcOq8B718dzZ34i85PeZzGaa0UD/s1600/change-direction-smooth.PNG" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEgXJDtFpkmxGB4XxOnseT1HYzcH12Pham642x1R0bltj5E5L3iCAYPK8kJvoybtmO3gSAAHHxcww-V3u68OL3aTWG6vWMtI2EnTsZQ432Gz_ov3frjHkWcOq8B718dzZ34i85PeZzGaa0UD/s1600/change-direction-smooth.PNG" /></a></div>
<br />
<br />
In this scenario I tested how the motor controller handles changing direction. I implemented a special control algorithm that can change direction smoothly. It seems that LeJOS motor controller does not handle this case correctly.<br />
<br />
<span style="font-size: large;"><b>Conclusion</b></span><br />
This comparison shows that LeJOS motor controller provides great control of the motor, in most cases it is better than Lego FW controller. But LeJOS controller has a few bugs that prevent its usage in some cases. So I currently try to implement alternative variant of motor controller.<br />
I have some suggestions to LeJOS motor controller authors:<br />
1. fix smooth acceleration behavior<br />
2. allow to turn off the stall detector<br />
3. fix the bug in lock method that prevent me from changing the lock power.<br />
<br />
<div class="separator" style="clear: both; text-align: center;">
</div>
</div>
Maximhttp://www.blogger.com/profile/14824654607202889952noreply@blogger.com0