Library for MPU-6050

Here is a C++ class for reading data from an MPU-6050 and send it to a computer using serial communication. This code is meant to be used in a specific order. First, the MPU-6050 range for the accelerometer and gyroscope must be set using the MPU6050.Initialize(frq, acc_rang, gyro_range) function, which also sets up timer 1 for acquiring data with the specified frequency. The highest frequency I have tested to work is 500 Hz – maybe the code can be improved? For this to work the serial connection must be started with a “Serial.begin(115200);”

When you are ready for recording data, that is when the serial connection is up and running the data collection can start: MPU6050.start(). Now timer 1 is started and data will be filled into a buffer. This buffer must be emptied using MPU6050.write() or MPU6050.print(). The first function sends binary data via the serial connection in the format of the struct DataFormat. The print function prints the numbers in a readable format in the serial monitor – it is mainly for debugging. The write or print functions must be executed at least once for each sample. Too frequent is not a problem, but if the buffer is not emptied at the same pace it is filled we have a problem. In that case an error indicated in the data. Another limiting factor is the data transfer over I2C – if the transfer of one sample is not complete before time is up for the next another error is generated. The error codes are:

  • ax = 32767, ay = 32767, az = 1: I2C communication did not finish before next sampling request. Solution: increase I2C communication frequency or lower the sampling frequency.
  • ax = 32767, ay = 32767, az = 2: Problem sending data fast enough. Solution: lower the sampling frequency or increase baud rate.

Data logging can be stopped using MPU6050.stop().

/* Documentation

#ifndef MPU6050_h
#define MPU6050_h

// Decide to print messages or binary numbers
//#define DEBUGMODE

#include <Arduino.h>
#include <inttypes.h>  // uint8_t is defined here

// Registers for the MPU-6050
#define ACCEL_XOUT_H 0x3B
#define MPUADR 0x68 // I2C address of the MPU-6050 (01101000)
#define WHO_AM_I 0x75 // [6:1] is normally equal to 0x68
#define POWERMANAGE 0x6B // Power management register

/* MPU 6050 registers: */
#define GYRO_CONFIG 0x1B
  //bits [7 6 5] for self test, 
  //       [4 3] for range: 0: 250 1: 500 2: 1000 3: 2000 [deg/s^2]
#define ACCEL_CONFIG 0x1C
  //bits [7 6 5] for self test,
  //       [4 3] for range: 0: 2g, 1: 4g, 2: 8g, 3: 16g [9.81*m/s^2]

/*   Define the size of the buffer. The larger the better but
 *   there must be some space left for other things. 24 seems
 *   sufficient for sampling rates up to 400 Hz
#define BUFFERSIZE 24
/*   The accelerometer data is read into this struct byte by
 *   byte. The accelerometer operates in big endian, therefor,
 *   the reversed order. The program that reads the SD-card 
 *   must use this data format in order to interpret it 
 *   correctly.
namespace MPU6050Lib{
struct DataFormat{
  int16_t gz;
  int16_t gy;
  int16_t gx;
  int16_t temp;
  int16_t az;
  int16_t ay;
  int16_t ax;
  uint8_t sync = 1;

/*  This structure is used for constructing the ring buffer.
 *  It also contains a variable to keep track on the progress 
 *  of reading and saving data.
struct DataBuffer{
  DataFormat data;
  uint8_t stat;
  DataBuffer *next;

class MPU6050Class{
   * Use this function to set up this class. Inputs:
   * * frequency    The sampling frequency
   * * accelRange   Accelerometer setting: 0: +-2g, 1: +-4g, 
   *                2: +-8g, 3: +-16g [9.81*m/s^2]
   * * gyroRange    Gyroscope setting: 0: +-250 1: +-500 2: +-1000 
   *                3: +-2000 [deg/s^2]
  void Initialize(uint16_t frequency, uint8_t accelRange, uint8_t gyroRange);
    * This function will start the timer interrupt.
  void start();
   * Call this function to read new data into the ring buffer.
   * Normally it should be called from a time interrupt to 
   * ensure constant sampling rate.
  uint8_t read();

   * Call this function to retrieve the oldest reading from 
   * the ring buffer. This function will light the red LED if
   * there is new data. It will turn it off if waiting.
   * Output: 1 if there was data in the buffer that has not 
   * already been retrieved. 0 if waiting for data.
  uint8_t write();
   *  Function for sending data via serial. Only for debugging.
   *  It will print the oldest data reading as formatted text.
   *  This function will light the red LED if there is new data. 
   *  It will turn it off if waiting for new data.
  uint8_t print();
   * Stop recording and empty buffers. Make ready for another
   * sampling.
  void stop();
  DataBuffer B[BUFFERSIZE];
  DataBuffer *Bf, *Be;
  #ifdef DEBUGMODE
  volatile uint8_t BufferStat = 0;

// Declare an instance of this class for external use.
// It is defined in MPU6050.cpp
extern MPU6050Class MPU6050;

// Reduce the amount of typing in the main program
using MPU6050Lib::MPU6050;


And here are the function definitions:

#include "MPU6050.h"

#define STATE_EMPTY      0
#define STATE_FULL       2

namespace MPU6050Lib{

#include "Arduino.h"
extern "C"{
#include "FastWire.h"

  // Set up the buffers to point at each other so that they form a circle
  for (int8_t i=BUFFERSIZE; i>0; i--){
    B[i-1].next = &B[i];
  B[BUFFERSIZE-1].next = B;
  Bf = B;
  Be = B;


void MPU6050Class::Initialize(uint16_t frequency, uint8_t accelRange, uint8_t gyroRange){
  // activate internal pullups for twi.
  digitalWrite(SDA, 1);
  digitalWrite(SCL, 1);
  /* Set the frequency of TWI communication. See page 292, 293 
   * and 266 in the manual */
  TWBR = 72; // 100 kHz
  //TWBR = 32; // 200 kHz
  //TWBR = 12; // 400 kHz
  // Wake up the accelerometer
  #ifdef DEBUGMODE
  Serial.println("sending power msg");
  FastWireSend(POWERMANAGE, 0);
  // Check the connection to the accelerometer
  uint8_t buf = 0;
  FastWireRead(WHO_AM_I, &buf, 1);
  while (uint8_t j = FastWireIsBusy()){
    // Wait for I2C to finish before comparing
    // Serial.println(j);
  if(buf == MPUADR){
    Serial.println("Accelerometer started...");
    Serial.println("Failed to start accelerometer");
    Serial.println(buf, HEX);
    while (1);
  if(buf != MPUADR){
  // Set the accelerometer range, accelRange = {0, 1, 2, 3}
  FastWireSend(ACCEL_CONFIG, (accelRange << 3));

  // Set the gyroscope range, gyroRange = {0, 1, 2, 3}
  FastWireSend(GYRO_CONFIG, (gyroRange << 3));
  // Set the sampling frequency
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  // Interrupt for data logging
  TCCR1B |= (1 << WGM12);   // Turn on CTC (Clear Timer on Compare Match)
  OCR1A = 62500/frequency;  // 500 Hz (125 = 62500 Hz / 500 Hz)
  TCCR1B |= (1 << CS12);    // 16MHz / 256 prescaler (62500 Hz)

void MPU6050Class::start(){
  Serial.println("Start recording");
  TIMSK1 |= (1 << OCIE1A); // Enable compare match A on timer 1

uint8_t MPU6050Class::read(){
  if (Be->stat == STATE_INPROGRESS){
    // Move to next buffer.
    Be->stat = STATE_FULL;
    if (FastWireIsBusy()){
      // If the I2C has not finished write an error code
      (Be->data).ax = 0x7FFF;
      (Be->data).ay = 0x7FFF;
      (Be->data).az = 1;
      (Be->data).temp = 0x7FFF;
      Be = Be->next;
      return 1;
    Be = Be->next;
  if (Be->stat == STATE_FULL){
    // This buffer must be emptied before we can continue
    (Be->data).ax = 0x7FFF;
    (Be->data).ay = 0x7FFF;
    (Be->data).az = 2;
    (Be->data).temp = 0x7FFF;
    return 2;
    FastWireRead(ACCEL_XOUT_H, (char*)&(Be->data), 14);
    Be->stat = STATE_INPROGRESS;
  return 0;
uint8_t MPU6050Class::write(){
  if (Bf->stat == STATE_FULL){
    // Send data to computer
    Serial.write((char*)(&Bf->data), sizeof(DataFormat));
    // Set the state of the current buffer element to empty
    Bf->stat = STATE_EMPTY;
    Bf = Bf->next;
    return 1;
    // Wait for data to be read
    return 0;
// This function is only for debugging
uint8_t MPU6050Class::print(){
  if (Bf->stat == STATE_FULL){
    Serial.print("\nBuffer Usage = "); Serial.println(BufferStat);
    Serial.print("Acc = ");
    Serial.print(Bf->; Serial.print(",\t ");
    Serial.print(Bf->data.ay); Serial.print(",\t ");
    Serial.print(Bf->; Serial.print("\t ");
    Serial.print("Gyro = ");
    Serial.print(Bf->data.gx); Serial.print(",\t ");
    Serial.print(Bf->; Serial.print(",\t ");
    Serial.print(Bf->data.gz); Serial.print("\t ");
    Serial.print("Temperature = ");
    Bf->stat = STATE_EMPTY;
    Bf = Bf->next;
    return 1;
    // Wait for data to be read
    return 0;

void MPU6050Class::stop(){
  // Stop the timer interrupt
  TIMSK1 = 0;

  // Empty the buffer
  Serial.println("\nEmptying buffer");
  // Make sure that the last empty buffer is set to empty
  Bf->stat = STATE_EMPTY;
  // Reset the pointers
  Bf = B;
  Be = B;
  Serial.println("\nRecording stoped");

MPU6050Class MPU6050;

byte onoff = 0;
  /* Blink GREEN led with half the frequency. It makes it possible 
   * to measure the frequency with an oscilloscope. Use it to
   * verify that the Arduino operates at the desired frequency.
   * No time stamp is saved - we rely on constant sampling
   * frequency.
  onoff = onoff ^ 1;
  digitalWrite(6, onoff);

  // Read data from the accelerometer and put it in the buffer;


Non blocking I2C read function

MPU6050, Arduino pro mini and SD card holder.

If you want to program a fast data logger with an Arduino, an SD card and a device that talks I2C you will run into problems with the Arduino Wire library, because it hangs up in a while loop as long as the I2C communication is ongoing. If you want to be fast you what to use that time for storing data on the SD card instead of waiting. So here is a c style program that will do the communication using interrupts only. It has only been tested with an MPU6050, and I cannot grantee that it will work with other devises – I can only say that it works for me…

Also, the functionality is very limited because I only needed to be able to send one byte of data and receive 14 from one predefined I2C divice. The read function is different from its equivalent in the Wire library because here you have to provide a buffer array to FastWireRead. Because the MPU6050 uses big endian and the Arduino and my Intel processor uses small endian the read function is programmed such that the first read byte of data goes to the last place in the buffer array. Therefore, the 16 byte integers comes in reverse order but in right endianness – without computational cost.

Take it – use it – it might work.

The header file:

#ifndef FASTWIRE_H
#define FASTWIRE_H
#include <inttypes.h>  // uint8_t is defined here

void FastWireSend(uint8_t reg, uint8_t dat);
void FastWireRead(uint8_t reg, uint8_t * buf, uint8_t n);
uint8_t FastWireIsBusy(void);


The function definitions

#include "FastWire.h"
#include <avr/io.h> // The I2C registers are defined here
#include <avr/interrupt.h>

// Status messeges from I2C
#define MT_START      0x08
#define MT_SLA_ACK    0x18
#define MT_DATA_ACK   0x28
#define MR_START      0x10  // repeated start
#define MR_SLA_ACK    0x40
#define MR_SLA_NACK   0x48
#define MR_DATA_ACK   0x50
#define MR_DATA_NACK  0x58

// These are specific to MPU6050 and should be changed if used with other components
#define SLA_W 0xD0 // 11010000 // (208)
#define SLA_R 0xD1 // 11010001 // (209)

static uint8_t I2Cwritebuffer[2];
static volatile uint8_t I2Cwrite_n = 0;
static volatile uint8_t I2Cwrite_i = 0;
static volatile uint8_t I2Cwrite_sendstop = 0;
static uint8_t * I2Creadbuffer;
static volatile uint8_t I2Cread_i = 0;
static volatile uint8_t I2Cbusy = 0;

void FastWireSend(uint8_t reg, uint8_t dat){
  // Wait for free line
  while(I2Cbusy != 0);
  while(TWCR & (1 << TWSTO));
  I2Cwritebuffer[0] = reg;
  I2Cwritebuffer[1] = dat;
  I2Cwrite_n = 2;
  I2Cwrite_i = 0;
  I2Cwrite_sendstop = 1;
  I2Cbusy = 1;
  // Initiate the I2C comunication
  TWCR =(1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);

void FastWireRead(uint8_t reg, uint8_t * buf, uint8_t  n){
  // Wait for free line
  while(I2Cbusy != 0);
  while(TWCR & (1 << TWSTO));
  I2Cwritebuffer[0] = reg;
  I2Cwrite_n = 1;
  I2Cwrite_i = 0;
  I2Cwrite_sendstop = 0;
  I2Creadbuffer = buf;
  I2Cread_i = n-1;
  I2Cbusy = 1;
  // Initiate the I2C comunication
  TWCR =(1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);

uint8_t FastWireIsBusy(void){
  return I2Cbusy;
 The interrupt that does all the work
  switch(TWSR & 0xF8){
    case MT_START:
      // Send the slave address and write bit
      TWDR = SLA_W;
      I2Cbusy = 2;
      TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
    case MT_SLA_ACK:
      I2Cbusy = 3;
      // Send the data in the transmit buffer
      TWDR = I2Cwritebuffer[I2Cwrite_i];
      I2Cwrite_i ++;
      TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
    case MT_DATA_ACK:
      I2Cbusy = 4;
      // Send more data or send stop signal
      if (I2Cwrite_i < I2Cwrite_n){
        TWDR = I2Cwritebuffer[I2Cwrite_i];
        I2Cwrite_i ++;
        TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
        if (I2Cwrite_sendstop){
          // Stop the transmission
          TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO) | (1<<TWIE);
          I2Cbusy = 0;
          // Start recieving data by sending a start signal
          TWCR =(1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);
          I2Cbusy = 1;
    case MR_START:
      TWDR = SLA_R;
      I2Cbusy = 5;
      TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
    case MR_SLA_ACK:
      I2Cbusy = 6;
      // Ask for the first uint8_t of data
      if (I2Cread_i == 0){
        // send NACK
        TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
        TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE);
 //   case MR_SLA_NACK:
 //     I2Cbusy = 8;
 //     break;
    case MR_DATA_ACK:
      I2Cbusy = 7;
      // Save data in buffer
      I2Creadbuffer[I2Cread_i] = TWDR;
      // In case there is more data to read, ask for more
      if (I2Cread_i == 1){
        // send NACK
        TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
        TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE);
    case MR_DATA_NACK:
      // Save the lase uint8_t
      I2Creadbuffer[I2Cread_i] = TWDR;
      // End the transmission
      TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN) | (1<<TWIE);
      I2Cbusy = 0;
      I2Cbusy = TWSR;


Numerical differentiation

In the numerical world, points represent measured signals and graphs of mathematical functions. Often, the points will be arranged in two arrays: one for the first axis (x) and another for the second (y). The arrays have the same amount of numbers, in this example n=11. I will also introduce an integer number i which is used for indexing into x and y. For illustrations in this post a third order polynomial has been used, it is y = f(x) = 0.1\cdot\left((x-1)(x-7)(x-12)\right)+9.

In order to estimate the derivative we will first have to recall the definition of the differential coefficient:

(1)   \begin{equation*} \frac{dy}{dx}=\lim_{\Delta x\to 0}\frac{f(x+\Delta x)-f(x)}{\Delta x}\end{equation*}

Forward difference

In numerical differentiation you care more about what the right side of the above equation says because we can approximate that calculation using the numbers in x and y – if we forget about the limit. The numerical approximation is

(2)   \begin{equation*} \frac{f(x+\Delta x)-f(x)}{\Delta x} = \frac{y_{i+1}-y_{i}}{x_{i+1}-x_i}\end{equation*}

However, because I left out the limit the equation is only an approximation of what the slope of the curve is in point i. Look at the figure; it illustrates the difference between the approximation and the analytical solution. This kind of numerical differentiation is called forward difference.

The slope of a third order polynomial
First order numerical differentiation. The green line indicates the analytically calculated slope and the red is the numerical solution.

It is worth noting that I am using two points for each calculation. Hence, when I want to calculate the slope of the last point I would have to use index i=n and i=n+1 which is not possible because there is no n+1. To include the last point you can use i=n-1 and i=n for the calculation. This is called backward difference.

Central difference

If you want to be more accurate, you can use central difference. The calculation is very similar, just instead of i and i+1 I am going to use i-1 and i+1:

(3)   \begin{equation*} \frac{dy}{dx} \approx \frac{y_{i+1}-y_{i-1}}{x_{i+1}-x_{i-1}}\end{equation*}

Second order numerical differentiation, central difference.

The drawback of the central difference is that the slope cannot be calculated for the first and the last point. If you want to know the slop at those points, you have to use forward and backward difference.

Another option to achieve second order accuracy is to evaluate the slope midway between i and i+1. This way, the slope will be calculated for each segment of the curve.

(4)   \begin{equation*} \frac{dy}{dx} \approx \frac{y_{i+1}-y_i}{x_{i+1}-x_i}\end{equation*}

The above calculation is valid at

(5)   \begin{equation*}x=0.5\cdot(x_i+x_{i+1})\end{equation*}

Central difference evaluated mid-way between two x-values.

Kinematic constraints

Machine parts


How machine parts attach to each other can be described by algebraic equations. This is useful if you want to study the function of for example a crane. Three parts are shown in the drawing below: part 1 is a wall, part 2 is a support rod and part 3 is a beam. Each of them are described by a length and a direction or in other words three vectors expressed as

(1)   \begin{equation*}S_1 = l_1\cdot \left[\begin{matrix} \cos{a_1}\\\sin{a_1} \end{matrix} \right],\ S_2 = l_2\cdot \left[\begin{matrix} \cos{a_2}\\\sin{a_2} \end{matrix} \right],\ S_3 = l_3\cdot \left[\begin{matrix} \cos{a_3}\\\sin{a_3} \end{matrix} \right] \end{equation*}

If this system is assembled the sum S_1 - S_2 + S_3 must equal zero. This implies that not all lengths and angles can be chosen freely. In fact, if body 1 is fixed at a_1=90^\circ and the lengths are l_1=0.7, l_2=1.2 and l_3=1 then a_2 and a_3 depends on the rest of the numbers, let’s say they are unknowns. The question is what are those two angles supposed to be in order to satisfy the constraint equation

(2)   \begin{equation*} \Phi = 0 = S_1 - S_2 + S_3 \end{equation*}

It is possible to solve this vector equation using the law of cosine for this example. However, I would like to do it in a more general manner.

First, I’m going to calculate how far the vectors are from forming a closed circuit if I choose the numbers

(3)   \begin{eqnarray*}S_1 =0.7\cdot \left[\begin{matrix} \cos{90}\\\sin{90} \end{matrix} \right],&S_2 = 1.2\cdot \left[\begin{matrix} \cos{20}\\\sin{20} \end{matrix} \right],&S_3 = 1.0\cdot \left[\begin{matrix} \cos{-10}\\\sin{-10} \end{matrix} \right] \\ S_1 =\left[\begin{matrix}0\\0.7 \end{matrix} \right], & S_2 = \left[\begin{matrix} 1.1276\\0.4104\end{matrix} \right],& S_3 = \left[\begin{matrix} 0.9848\\-0.1736 \end{matrix} \right] \end{eqnarray*}

Then the value of \Phi can be evaluated to [-0.1428, 0.1159]^T, which is not that close to [0,0]^T. I have to adjust the angles such that this error becomes smaller and I want to do it in a smart way so that it will also work for more complicated systems. The idea is to investigate all the unknown variables one at a time and figure out how that changes the system. So first change a_1 by \delta=0.001 radians considering the entire set of constraint equations

(4)   \begin{equation*} \delta S_2 = \frac{\left( S_1 - l_2\cdot \left[\begin{matrix} \cos{a_2+\delta}\\\sin{a_2+\delta} \end{matrix} \right] +S_3 \right) - \left(S_1 - S_2 + S_3\right)}{\delta}  = \left[\begin{matrix} 0.4110\\ -1.1274 \end{matrix}\right]\end{equation*}

The equation can be simplified by removing S_1 and S_3 because they doesn’t depend on a_2. The result of eq. 4 can be interpreted as the amount that S_2 will change if a_2 is changed by a unit (the calculations are in radians) and it is worth mentioning that it is only valid for small changes in a_2 as we are talking about a linearization. I do the same with a_3:

(5)   \begin{equation*} \delta S_3 = \frac{l_3\cdot \left[\begin{matrix} \cos{a_3+\delta}\\\sin{a_3+\delta} \end{matrix} \right] - \left(S_3\right)}{\delta}  = \left[\begin{matrix} 0.1732\\ 0.9849 \end{matrix}\right]\end{equation*}

Partly assembled crane

Now the good question is how much a_2 and a_3 must be changed to get \Phi closer to zero? We know the distance between the open connection, it is \Phi so if we find a linear combination of \delta S_2 and \delta S_3 that equals \Phi that might be useful. These two equations with two unknown changes in angles is

(6)   \begin{equation*} \Phi = \delta S_2\cdot \delta a_2 + \delta S_3\cdot \delta a_3 \end{equation*}

Solving this equation yields \delta a_2=-15.3495^\circ and \delta a_3 = -10.8268^\circ. Then the angles must be updated and S_2, S_3 and \Phi recalculated:

(7)   \begin{eqnarray*} a_2 &=& 20 + 15.3495 = 35.3495\\ a_3 &=& -10 + 10.8268 = 0.8268 \\ S_2 &=& \left[\begin{matrix} 0.9788 \\ 0.6943 \end{matrix} \right] \\ S_3 &=& \left[\begin{matrix} 0.9999 \\ 0.0144 \end{matrix} \right] \\ \Phi&=&  \left[\begin{matrix} 0.0211 \\ 0.0202 \end{matrix} \right] \end{eqnarray*}

So the new values of the variables improves the solution, but \Phi is not zero therefor the calculations are repeated from eq.4. The second time \Phi = [-0.00082, 0.00035]^T and in the third iteration we are really close to the analytic solution: \Phi = [ 0.00000, 0.00000]^T with the angle a_2 = 33.6124^\circ and a_3 = -2.0467^\circ.

All of this can be generalized by introducing the Jacobian often named \Phi_q because it is the partial derivative of the constraint equations \Phi with respect to the generalized coordinates q = [a_2, a_3]. In many cases the Jacobian can be established analytically with some effort. The way of updating a_2 and a_3 is named Newton-Rapson after the inventors of the method.

l1 = 0.7;
l2 = 1.2;
l3 = 1.0;

% Defined angle
a1 = pi/2;

% Initial guess on the tow other angle
a2 = 20*pi/180;
a3 = -10*pi/180;

% Calculate the vectors
S1 = l1 *[cos(a1);sin(a1)];
S2 = l2 *[cos(a2);sin(a2)];
S3 = l3 *[cos(a3);sin(a3)];

% Sum all the vectors to calculate if they form a triangle. If Phi is zero,
% the crane is assembled correctly.
Phi = S1 - S2 + S3;
fprintf('Phi = [%8.5f; %8.5f]\n', Phi(1), Phi(2));

for i=1:3
  % Let's try to predict what happens if the two "free" angles are adjusted
  delta = 0.001;
  dS2 = (S1 -l2 *[cos(a2+delta);sin(a2+delta)] + S3 - (S1 - S2 + S3))/delta;
  dS3 = (l3 *[cos(a3+delta);sin(a3+delta)] - S3)/delta;
  %% Guess a better prediction for A2 and A3
  % Phi = dS2*da2 + dS3*da3 translated to matrix form:
  dS = [dS2, dS3];
  da = dS\Phi;
  da2 = da(1); da3 = da(2);

  % Update the variables and repeat the calculation of Phi
  a2 = a2 - da2;
  a3 = a3 - da3;
  fprintf('a2 = %8.4f, a3 = %8.4f\n', a2*180/pi, a3*180/pi);

  S1 = l1 *[cos(a1);sin(a1)];
  S2 = l2 *[cos(a2);sin(a2)];
  S3 = l3 *[cos(a3);sin(a3)];
  Phi = S1 - S2 + S3;
  fprintf('Phi = [%8.5f; %8.5f]\n', Phi(1), Phi(2));


Vector graphics with correct font in LaTeX

In this post I’m going to present one out of 1000 ways to get nice vector graphics and fonts in LaTeX documents.

  1. First step is to draw a really good figure in your favorite program. For some unknown reason I have been using Visio for a while. Here’s an example drawing:

    Example on a Vector drawing with text formatted for LaTeX
  2. Add text to the drawing using LaTeX formatting syntax.
  3. Save the drawing in .svg format. It can be done fast by pressing this combination of buttons after each other: Alt, F, A, Tab, S, S, Enter
  4. Open the .svg with Inkscape.

    The figure opened in Inkscape
  5. Save the figure as an .eps file. You can use the shortcut Ctrl+Shift+S and select .eps in the drop-down menu.

    The Save-As dialog box in Inkscape
  6. Select Omit text in EPS and create LATeX file.

    The Save-As dialogue box in Inkscape
  7. Include the figure in your LaTeX document:
      \caption{Drawing of a single DOF linear system without damping and friction.}

    If you look into the .eps_tex file you will find more guidance on how to adjust figure size.

  8. And this is how it looks in the PDF:

    The figure as it looks when compiled in LaTeX

It is very likely that you will have to tweak the position of the text a bit to have it in the right place, so you might have to repeat the process from step 3 a couple of times.

Leap-frog vs. Forward Euler

In this post I’m showing how to program a Leap-Frog integrator. Its performance of it is compared to the Forward-Euler integration method and an analytical solution.

For the testing, a very simple mechanical system is considered. It consists of a body with mass m = 2, a spring with stiffness k = 4 and a damper with damping c = 1.

The body acceleration is calculated at a given time step by evaluating the equation of motion (Newton’s second low) given the instantaneous position and velocity

\ddot{x}_{i} = a(x, \dot{x})

Based on that acceleration the velocity can be calculated half a step in advance

\dot{x}_{i+0.5} = \dot{x}_{i-0.5} + \ddot{x}_{i}\cdot h

Where h is the step size often measured in seconds. Now the position to the next full time step can be calculated

x_{i+1} = x_{i} + \dot{x}_{i+0.5}\cdot h

But we also need to calculate the velocity at the next full time step in order to be able to calculate the next acceleration. The next velocity can be estimated by this first order finite difference (it is only a true finite difference when s =1)

\dot{x}_{i+1} = \dot{x}_{i+0.5}+(\dot{x}_{i+0.5}-\dot{x}_{i})\cdot s

where s is a stability constant which might take any value in the range from -0.25 to 1.

And now to the programming

The test case is a simple one-dimensional system consisting of a mass, spring and damper with the properties:

m = 2;
k = 40;
c = 1;

Define a function for calculation of acceleration given the position q and velocity \dot{q}. The resulting force on the mass is simply F = -k\cdot q-c\cdot \dot{q}, then, the acceleration can be calculated using Newtons second law.

function ddq = Acceleration(q, dq)
    ddq = (-k*q -c*dq)/m;

The solution to the differential equation will be calculated to the discrete time given here:

t = linspace(0, 3, 40);
% the time step size
h = t(2)-t(1);
% Vectors for the results are prealocated here
euler = struct('x',zeros(size(t)),'dx',zeros(size(t)),'ddx',zeros(size(t)));
lf = struct('x',zeros(size(t)),'dx',zeros(size(t)),'ddx',zeros(size(t)));

To find a specific solution one must specify some boundary conditions. In this case the initial position and velocity of the particle is given.

x0 = 1;
dx0 = 0.5;
euler.x(1) = x0;
lf.x(1) = x0;
euler.dx(1) = dx0;
lf.dx(1) = dx0;

% Aclculate the accelerations given the initail conditions
euler.ddx(1) = Acceleration(euler.x(1), euler.dx(1));
lf.ddx(1) = Acceleration(lf.x(1), lf.dx(1));

% For the Leap-Forg the velocity is calculated half a time-step in advance.
% This half-step must be initailized as well.
lf_dx = lf.dx(1) + lf.ddx(1) * 0.5 * h;

The stability factor is artificial damping. Funny enough, the more damping you put into the model, the more stability is required, hence, a lower stability factor should be selected. The value can be any number in the range from 1 to -0.25. Choosing low numbers compromise the frequency response. Choosing stability = -0.25 is almost equivalent to the average approach.

stability = 0.62;

for i=1:length(t)-1
    % Forward euler integration
    euler.dx(i+1) = euler.dx(i) + euler.ddx(i)*h;
    euler.x(i+1) = euler.x(i) + euler.dx(i)*h;
    euler.ddx(i+1) = Acceleration(euler.x(i+1), euler.dx(i+1));
    % Leap-Frog integration
    lf.x(i+1) = lf.x(i) + lf_dx*h;
    % Velocity at full step
    lf.dx(i+1) = lf_dx + (lf_dx - lf.dx(i)) * stability;

    lf.ddx(i+1) = Acceleration(lf.x(i+1), lf.dx(i+1));
    lf_dx = lf_dx + lf.ddx(i+1)*h;

% Analytic solution
omega_n = sqrt(k/m);
xi = c / (2*sqrt(k*m));
omega_d = omega_n*sqrt(1-xi^2);
x = exp(-xi*omega_n*t) .* (x0*cos(omega_d*t) + (dx0 + xi*omega_n*x0)./omega_d*sin(omega_d*t));

 The Results

It can be seen that for this simple mechanical system the Leap-Frog is much better than Forward-Euler. It is almost identical to the analytic solution – but in more complicated systems, the Leap-Frog is not as convincing.

Arduino class for HX711

Mechanical engineers need strain gauges! A cheap and (with this class) easy way is to use an Arduino and up to six HX711 analog to digital converters for reading six or 12 strain gauges. This class will write data directly to the serial port each time the ReadPort(mode) function is called. So it is just a matter of calling that function whenever you need to read the HX711’s. Of cause the computer must be ready to receive and store data. Take a look at my previous post to see how this can be done with MATLAB.

The code uses two dirty tricks: the first is the fact that the bytes are ordered the same way in the RAM as they are declared in the class. That means that even though all data in the class is declared as 24 8 bit unsigned integers it can be interpreted as 6 32 bit signed integers. This allows all data to be send using just one Serial.write(…). Then, what the computer is getting is:

  • uint8: read mode which can take the numbers 1, 2 or 3
    • 1 = port A, gain = 128
    • 2 = port B, gain = 32
    • 3 = port A, gain = 64
  • int32: Value from HX711 no. 1
  • int32: Value from HX711 no. 2
  • int32: Value from HX711 no. 3
  • int32: Value from HX711 no. 4
  • int32: Value from HX711 no. 5
  • int32: Value from HX711 no. 6

The second trick is to avoid using digitalRead(i) 6 times in a row. It is faster (but maybe less safe) to read the register that holds the digital values of the analog pins directly. That register is named PINC. But sorting the bits in the right order is a puzzle.

We have been able to acquire data with a sampling rate of 40 Hz. The limit of the HX711 is 80 Hz – Maybe the sampling rate will improve with a baud-rate higher than 9600.

/* A class for reading up to 6 HX711's at the same time.
 * So far this code has only been tested in one setup on an 
 * Arduino UNO. We cannot promise that it will work in other 
 * setups or that this code will work at all.
 * Created by
 * Jørgen Holm
 * Morten Haastrup
 * PINC is the internal input register for the analog pins 
 * on an Arduino. The value of this pin tells the status 
 * (HIGH or LOW) on all analog pins. Below, a debug example:

  outPins = PINC;

uint8_t outPins;
class HX711{
  /* A byte telling which mode is read. Default is reading
  the A port with gain = 128.*/
  uint8_t gain = 1;
  /* the bytes are ordered in little endian so that they
  can be red directly as 32 bit integers by an intel cpu. */
  uint8_t H1c, H1b, H1a, H1s;
  uint8_t H2c, H2b, H2a, H2s;
  uint8_t H3c, H3b, H3a, H3s;
  uint8_t H4c, H4b, H4a, H4s;
  uint8_t H5c, H5b, H5a, H5s;
  uint8_t H6c, H6b, H6a, H6s;
  uint8_t GetGain(){ return gain; };
  inline void SignBit(uint8_t bits){
    if(bits & 0x01) H1s = 0xFF;
    else H1s = 0;
    if(bits & 0x02) H2s = 0xFF;
    else H2s = 0;
    if(bits & 0x04) H3s = 0xFF;
    else H3s = 0;
    if(bits & 0x08) H4s = 0xFF;
    else H4s = 0;
    if(bits & 0x10) H5s = 0xFF;
    else H5s = 0;
    if(bits & 0x20) H6s = 0xFF;
    else H6s = 0;
  inline void ShiftA(){
    H1a = H1a << 1; H2a = H2a << 1; H3a = H3a << 1;
    H4a = H4a << 1; H5a = H5a << 1; H6a = H6a << 1;
  inline void ShiftB(){
    H1b = H1b << 1; H2b = H2b << 1; H3b = H3b << 1;
    H4b = H4b << 1; H5b = H5b << 1; H6b = H6b << 1;
  inline void ShiftC(){
    H1c = H1c << 1; H2c = H2c << 1; H3c = H3c << 1;
    H4c = H4c << 1; H5c = H5c << 1; H6c = H6c << 1;
  inline void InsertA(uint8_t bits){
    if (bits & 0x01) H1a |= 1;
    if (bits & 0x02) H2a |= 1;
    if (bits & 0x04) H3a |= 1;
    if (bits & 0x08) H4a |= 1;
    if (bits & 0x10) H5a |= 1;
    if (bits & 0x20) H6a |= 1;
  inline void InsertB(uint8_t bits){
    if (bits & 0x01) H1b |= 1;
    if (bits & 0x02) H2b |= 1;
    if (bits & 0x04) H3b |= 1;
    if (bits & 0x08) H4b |= 1;
    if (bits & 0x10) H5b |= 1;
    if (bits & 0x20) H6b |= 1;
  inline void InsertC(uint8_t bits){
    if (bits & 0x01) H1c |= 1;
    if (bits & 0x02) H2c |= 1;
    if (bits & 0x04) H3c |= 1;
    if (bits & 0x08) H4c |= 1;
    if (bits & 0x10) H5c |= 1;
    if (bits & 0x20) H6c |= 1;
  void ReadPort(uint8_t nextPort){
    // Reset all most significant bytes
    H1a = H2a = H3a = H4a = H5a = H6a = 0;
    // Power up the HX711's
    digitalWrite(7, LOW);
    // Whait for them to become ready (Mask out the 2 MSB as they are undefined)
    while(PINC & 0x3F);
    // Wait minimum 0.1 microsecond
    // Give 24 pulses on the clock pin to get the data out
    digitalWrite(7, HIGH);
    outPins = PINC;
    digitalWrite(7, LOW);
    for(uint8_t i=7; i != 0; i--){
       digitalWrite(7, HIGH);
       outPins = PINC;
       digitalWrite(7, LOW);
    for(uint8_t i=8; i != 0; i--){
       digitalWrite(7, HIGH);
       outPins = PINC;
       digitalWrite(7, LOW);
    for(uint8_t i=8; i != 0; i--){
       digitalWrite(7, HIGH);
       outPins = PINC;
       digitalWrite(7, LOW);
    /* Send the remaining pulses for setting the gain for next reading
     * gain = 1 : Port A (Gain = 128)
     * gain = 2 : Port B (Gain =  32)
     * gain = 3 : Port A (Gain =  64) */
    for (uint8_t i = 0; i < nextPort; i++) {
      digitalWrite(7, HIGH);
      digitalWrite(7, LOW);

    // Send data to the computer
    Serial.write((byte*)this, sizeof(HX711));

    // Then prepare for next go
    gain = nextPort;


Communication between Arduino and MATLAB


If you are tired of just creating simulations with MATLAB, you could build a motion base which you can control. This motion base is made of 6 linear actuators with potentiometers for measuring their extension, some motor h-bridges and an Arduino. However, in this example I’m just going to show how to send numbers to the Arduino and get some back. This is also a way to keep the communication synchronized.

First of all there must be a connection between the Arduino and MATLAB. I’m using the USB cable and serial communication. The MATLAB code for opening the serial communication is

obj = serial('COM4');
obj.DataBits = 8;
obj.StopBits = 1;
obj.BaudRate = 9600;
obj.Parity = 'none';
obj.Timeout = 1;

The Serial connection needs a little time to start up, therefore I have put a pause command right after the fopen. I also set the timeout to 1s because there is no need to wait 10s to find out if there is a problem with the connection.

To do the same with the Arduino put this line in the setup function:


The next thing to do is to synchronize the data transfer.  This can be achieve by sending some character from MATLAB to the Ardiuno and let the Arduino reply with another. It is assumed that the Ardino is already connected and running the setup function when the MATLAB program is started.

% Send a random character to the Arduino to get a reply
fwrite(obj, 'k', 'char');

% Wait for the reply
while (obj.BytesAvailable < 1)
    fwrite(obj, 'k', 'char');
% Read the reply and take measures
HandShake = fread(obj, 1, 'char');
if HandShake == 'b'
    fwrite(obj, 'a', 'char');
    disp('Something wrong with the connection')
fprintf('HandShake %s\n', HandShake);

% The Arduino needs a small gap to prepare itself


And the Arduino handshake

byte handshake = 'x';
while (handshake != 'a'){
  while(Serial.available() < 1);
  handshake =;

In this case I would like to send 6 unsigned 16 bit integers to the Arduino. These numbers are the desired length of the 6 legs on the motion base. Then I want the Arduino to reply with the actual measured lengths. The MATLAB part of the work looks like this:

% Send to Arduino
fwrite(obj, s, 'int16');

% Wait for reply
while obj.BytesAvailable < 12

% Read the numbers recieved from the Arduino
x = fread(obj, 6, 'int16');

And this is how the Arduino replies

// Wait for data
while(Serial.available() < 12);

/* Read in the 12 bytes from the Serial buffer and 
 * assemble them to 16 bit integers. 
Serial.readBytes((byte*)s, 12);

// Do something with the numbers
for(int16_t i = 0; i < 6; i++){
  x[i] = s[i] + 1;

// Blink the led as many times as indicated in the recieved
for(uint8_t i = s[0]; i > 0; i--){
  digitalWrite(13, HIGH);
  digitalWrite(13, LOW);

// Send back the nubers in 12 bytes
Serial.write((byte*)x, 12);

note that s and x are declared as arrays of int16_t arrays. However when we read and write we just treat them as arrays of bytes that are 12 bytes long each.


Numerical integration


This post is an introduction to some selected methods for numerical integration. There are many methods and algorithms for numerical integration of Ordinary Differential Equations (ODE). Here some for solving equations like

(1)   \begin{eqnarray*} \dot{x}&=&f(t,\ x)\\ \ddot{x}&=&f(t,\ x,\ \dot{x}) \end{eqnarray*}

are presented. Often in mechanical engineering, t denotes time, x position, \dot{x} velocity and \ddot{x} acceleration. All the methods requires that the initial conditions are known so that the function can be evaluated using those. The initial conditions are denoted t_0, x_0 and \dot{x}_0 if it is a second order equation.

The most straight forward integrators are explicit, because the function value for next time-step depends only on the current derivatives. Examples of explicit methods are

  • Forward Euler
  • Leap-frog
  • Runge-Kutta
  • Adams-Bashforth

The other category is implicit integrators:

  • Backward Euler
  • Newmark
  • Adams-Moulton

Forward Euler

Assume that the derivative at time step i is constant until time step i+1. Then the change in function value can be determined as the product of that time step h and the derivative \dot{x}_i

(2)   \begin{eqnarray*} \dot{x}_i &=& f(t_i,\ x_i)\\ x_{i+1}&=&x_i+\dot{x}_i\cdot h \end{eqnarray*}

This method also works for second order systems:

(3)   \begin{eqnarray*} \ddot{x}_i &=& f(t_i,\ x_i,\ \dot{x}_i)\\ \dot{x}_{i+1} &=& \dot{x}_i+\ddot{x}_i\cdot h\\ x_{i+1} &=& x_i+\dot{x}_i\cdot h \end{eqnarray*}

Backward Euler

The simplest implicit method one can think of. It is implicit because the calculation of x_{i+1} depends on \dot{x}_{i+1} which can only be calculated once x_{i+1} is known:

(4)   \begin{equation*} x_{i+1}=x_i+\dot{x}_{i+1}\cdot h \end{equation*}

For some systems like a liner spring-damper-mass system it is possible to obtain an analytic expression for solving the equation above. In other cases iterative methods can be used, for example the sequence

(5)   \begin{eqnarray*} x_{i+1}&=&x_i+\dot{x}_{i}\cdot h\nonumber\\ \dot{x}_{i+1}&=&f(t_{i+1},\ x_{i+1})\\ x_{i+1}&=&x_i+\dot{x}_{i+1}\cdot h \end{eqnarray*}

will approach a solution if the last two equations are evaluated several times in each time step.


This algorithm is intended especially for second order ODE’s, but can be used for first order as well. Second order accuracy is achieved by performing the first integration i half steps. This way, central difference is used in stead of the forward difference in the forward Euler.

In order to initialize this method the first half step must be calculated before entering the time integration loop:

(6)   \begin{eqnarray*} \dot{x}_{i-0.5}& =& \dot{x}_0 - \ddot{x}_0\cdot \frac{h}{2} \end{eqnarray*}

The time integration can then proceed

(7)   \begin{eqnarray*} \dot{x}_{i+0.5}& = &\dot{x}_{i-0.5} + \ddot{x}_i\cdot h\\ x_{i+1}&=& x_i + \dot{x}_{i+0.5} \cdot h\\ \dot{x}_{i+1}&=&\dot{x}_{i+0.5} + \left(\dot{x}_{i+0.5} - \dot{x}_{i}\right)\cdot s\\ \ddot{x}_{i+1}&=&f(t_{i+1},\ x_{i+1},\ \dot{x}_{i+1}) \end{eqnarray*}


where s is a stability constant which might take any value in the range
from -0.25 to 1. If this integration scheme is to be used for at first order ODE then the third line is left out.


The fourth order Runge-Kutta method is documented by Kreyszig (Advanced Engineering Mathematics 9^{th} edition p. 892). It is developed for first order ODE’s.

(8)   \begin{eqnarray*} k_1 &=& h\cdot f(t_i,\ x_i)\nonumber\\ k_2 &=& h\cdot f(t_i+\frac{1}{2}h,\ x_i+\frac{1}{2}k_1)\nonumber\\ k_3 &=& h\cdot f(t_i+\frac{1}{2}h,\ x_i+\frac{1}{2}k_2)\nonumber\\ k_4 &=& h\cdot f(t_i+h,\ x_i+k_3)\nonumber\\ x_{i+1}&=& x_i + \left(k_1+2k_2+2k_3+k_4\right)\cdot\frac{1}{6}\\ \dot{x}_{i+1}&=& f(t_{i+1},\ x_{i+1}) \end{eqnarray*}

In order to use this method for second order system a function that transforms a second order system to two first order is introduced:

(9)   \begin{eqnarray*} \dot{y} = \left[\begin{matrix} \ddot{x}\\\dot{x} \end{matrix} \right] = g(t,\ y) = \left[\begin{matrix} f(t,\ x,\ \dot{x})\\ \dot{x} \end{matrix} \right] \end{eqnarray*}

where y = \left[\dot{x}^T, x^T\right]^T.
Then the function g is integrated using the above mentioned procedure. I will show how to program this in another post.


Another option for solving second order ODE’s is to use Runge-Kutta-Nyström Kreyszig (Advanced Engineering Mathematics 9^{th} edition p. 906). It is

(10)   \begin{eqnarray*} k_1 &=& \frac{h}{2}\cdot f(t_i,\ x_i,\ \dot{x}_i)\nonumber\\ K &=& \frac{h}{2}\cdot\left(\dot{x}_i + \frac{1}{2}\cdot k_1\right)\nonumber\\ k_2 &=& \frac{h}{2}\cdot f(t_i+\frac{h}{2},\ x_i+K,\ \dot{x}_i+k_1)\nonumber\\ k_3 &=& \frac{h}{2}\cdot f(t_i+\frac{h}{2},\ x_i+K,\ \dot{x}_i+k_2)\nonumber\\ L&=&h\cdot \left(\dot{x}_i + k3\right)\nonumber\\ k_4 &=& \frac{h}{2}\cdot f(t_i+h,\ x_i+L,\ \dot{x}_i+2\cdot k_3)\nonumber\\ x_{i+1}&=& x_i + h\cdot \left(\dot{x} + \frac{1}{3}\left(k_1+k_2+k_3\right) \right)\\ \dot{x}_{i+1}&=& \dot{x}_i + \frac{1}{3}\left(k_1+2\cdot k_2+2\cdot k_3+k_4\right)\\ \ddot{x}_{i+1}&=& f(t_{i+1},\ x_{i+1},\ \dot{x}_{i+1}) \end{eqnarray*}

Adams-Bashforth and Adams-Moulton

Is a multi-step predictor-correcter algorithm. It is a multi-step method because it uses the history of calculated values of \dot{x} to predict the next value of x. It is a predictor-corrector method because it uses the derivative of the next step to calculate the value for next step.

The first problem with multi-step methods is that the first few steps need to be calculated with some other method. The other problem is that the derivatives are assumed to be smooth, which is not the case in contact problems.

The prediction of x_{i+1} is done by fitting a fourth order polynomial

(11)   \begin{eqnarray*} x(t)&=&k_4t^4+k_3t^3+k_2t^2+k_1t+k_0\\ \dot{x}(t)&=&4k_4t^3+3k_3t^2+2k_2t+k_1\nonumber\\ x(0)&=&x_i=k_0\nonumber\\ \dot{x}(0)&=&\dot{x}_i=k_1\nonumber\\ \dot{x}(-h)&=& \dot{x}_{i-1}=  -4k_4h^3 +3k_3h^2-2k_2h + \dot{x}_i\nonumber\\ \dot{x}(-2h)&=&\dot{x}_{i-2}= -32k_4h^3+12k_3h^2-4k_2h + \dot{x}_i\nonumber\\ \dot{x}(-3h)&=&\dot{x}_{i-3}=-108k_4h^3+27k_3h^2-6k_2h + \dot{x}_i\nonumber \end{eqnarray*}

These equations can be rewritten in matrix form and solved

(12)   \begin{eqnarray*} \left[ \begin{matrix} k_4h^3\\k_3h^2\\k_2h \end{matrix}\right]  = \frac{1}{24} \left[ \begin{matrix} -3& 3 &-1\\-20& 16& -4\\-36& 18& -4 \end{matrix}\right] \cdot \left[ \begin{matrix} \dot{x}_{i-1} - \dot{x}_i\\\dot{x}_{i-2} - \dot{x}_i\\\dot{x}_{i-3} - \dot{x}_i \end{matrix}\right] \end{eqnarray*}

by substituting into 11 an expression for the next value is obtained:

(13)   \begin{eqnarray*} x_{i+1}^*=x(h)=x_i+\frac{h}{24}\left(55\dot{x}_i-59\dot{x}_{i-1}+37\dot{x}_{i-2}-9\dot{x}_{i-3}\right)  \end{eqnarray*}

Equation 13 is known as the Adams-Bashforth method. It can be improved by appending an correcter step which is deducted in a similar fashion, where \dot{x}_{i-3} is replaced by \dot{x}_{i+1}. The corrector step is

(14)   \begin{eqnarray*} \dot{x}_{i+1} &=& f(t_{i+1},\ x_{i+1}^*)\\ x_{i+1} &=& x_i + \frac{h}{24}\left(9\dot{x}_{i+1}+19\dot{x}_{i}-5\dot{x}_{i-1}+\dot{x}_{i-2}\right) \end{eqnarray*}