In this page, I will discuss the coding process followed to achieve the final project goals.

Useful resources :

Arduino IDE Libraries used :

Page content :

Click here to download the full code.

Introduction:

The aim of my program is to measure the knee angle. For this purpose I used two MPU6050, which is an inertial measurement unit (IMU) of six degrees of freedom (6DOF) that combines a 3-axis accelerometer and a 3-axis gyroscope.

STEP 1: Pitch (Y-axis) and Roll (X-axis) angles calculation

First of all, I have connected the MPU6050's to Arduino UNO for testing and uploaded the code found from this site. After that, I made the required changes to run the code on ATtiny84 as following:


/* http://www.electronoobs.com/eng_arduino_tut76.php
 *  http://www.youtube.com/c/electronoobs/eng_arduino_tut76.php
 * This is an example where we configure te data of the MPU6050
 * and read the Acceleration data and print it to the serial monitor
 * Arduino pin    |   MPU6050
 * 5V             |   Vcc
 * GND            |   GND
 * A4             |   SDA
 * A5             |   SCL
 */

//#include "Wire.h"
//#include "I2Cdev.h"
#include <MPU6050.h>

#include <TinyWireM.h>
#include <SoftwareSerial.h>
#ifdef DEBUG
#endif

//int tx=1;
//int rx=0;

//Gyro Variables
float elapsedTime, time, timePrev;        //Variables for time control
int gyro_error=0;                         //We use this variable to only calculate once the gyro data error
float Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data read 
float Gyr_rawX2, Gyr_rawY2, Gyr_rawZ2;
float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value obtained with Gyro data
float Gyro_angle_x2, Gyro_angle_y2;     
float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error
float Gyro_raw_error_x2, Gyro_raw_error_y2;

//Acc Variables
int acc_error=0;                         //We use this variable to only calculate once the Acc data error
float rad_to_deg = 180/3.141592654;      //This value is for pasing from radians to degrees values
float Acc_rawX, Acc_rawY, Acc_rawZ;    //Here we store the raw data read 
float Acc_rawX2, Acc_rawY2, Acc_rawZ2;
float Acc_angle_x, Acc_angle_y;          //Here we store the angle value obtained with Acc data
float Acc_angle_x2, Acc_angle_y2;  
float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error
float Acc_angle_error_x2, Acc_angle_error_y2;

float Total_angle_x, Total_angle_y;
float Total_angle_x2, Total_angle_y2;


long accelX, accelY, accelZ;
float gyroX1, gyroY1, gyroX2, gyroY2, rotX, rotY, rotZ;
long accelX2, accelY2, accelZ2;

SoftwareSerial Monitor(PA1, PA0);

void setup() {   
Monitor.begin(9600);
  TinyWireM.begin();
  delay(1000); // Display title

  TinyWireM.beginTransmission(0x68);
  TinyWireM.write(0x6B);
  TinyWireM.write(0b00000000);
  TinyWireM.endTransmission();

  TinyWireM.beginTransmission(0x68);
  TinyWireM.write(0x1B);
  TinyWireM.write(0x00000000);
  TinyWireM.endTransmission();

  TinyWireM.beginTransmission(0x68);
  TinyWireM.write(0x1C);
  TinyWireM.write(0b00000000);
  TinyWireM.endTransmission();


  
  TinyWireM.beginTransmission(0x69);
  TinyWireM.write(0x6B);
  TinyWireM.write(0b00000000);
  TinyWireM.endTransmission();

  TinyWireM.beginTransmission(0x69);
  TinyWireM.write(0x1B);
  TinyWireM.write(0x00000000);
  TinyWireM.endTransmission();

  TinyWireM.beginTransmission(0x69);
  TinyWireM.write(0x1C);
  TinyWireM.write(0b00000000);
  TinyWireM.endTransmission();

    if(acc_error==0)
  {
    for(int a=0; a<200; a++)
    {
      TinyWireM.beginTransmission(0x68);
      TinyWireM.write(0x3B);                       //Ask for the 0x3B register- correspond to AcX
      TinyWireM.endTransmission(false);
       TinyWireM.requestFrom(0x68,6); 
          Acc_rawX=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; //each value needs two registres
          Acc_rawY=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
          Acc_rawZ=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
      TinyWireM.endTransmission(true);

      TinyWireM.beginTransmission(0x69);
      TinyWireM.write(0x3B);                       //Ask for the 0x3B register- correspond to AcX
      TinyWireM.endTransmission(false);
      TinyWireM.requestFrom(0x69,6); 
          Acc_rawX2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; //each value needs two registres
          Acc_rawY2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
          Acc_rawZ2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
      TinyWireM.endTransmission(true);

      
      /*---X---*/
      Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg));
      Acc_angle_error_x2 = Acc_angle_error_x2 + ((atan((Acc_rawY2)/sqrt(pow((Acc_rawX2),2) + pow((Acc_rawZ2),2)))*rad_to_deg));
      /*---Y---*/
      Acc_angle_error_y = Acc_angle_error_y + ((atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg)); 
      Acc_angle_error_y2 = Acc_angle_error_y2 + ((atan(-1*(Acc_rawX2)/sqrt(pow((Acc_rawY2),2) + pow((Acc_rawZ2),2)))*rad_to_deg));    
      
      if(a==199)
      {
        Acc_angle_error_x = Acc_angle_error_x/200;
        Acc_angle_error_y = Acc_angle_error_y/200;
        acc_error=1;

        Acc_angle_error_x2 = Acc_angle_error_x2/200;
        Acc_angle_error_y2 = Acc_angle_error_y2/200;
        acc_error=1;
      }
    }
  }//end of acc error calculation   


/*Here we calculate the gyro data error before we start the loop
 * I make the mean of 200 values, that should be enough*/
  if(gyro_error==0)
  {
    for(int i=0; i<200; i++)
    {
      TinyWireM.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
      TinyWireM.write(0x43);                        //First adress of the Gyro data
      TinyWireM.endTransmission(false);
      TinyWireM.requestFrom(0x68,4);           //We ask for just 4 registers 
          Gyr_rawX=TinyWireM.read()<<8|TinyWireM.read();     //Once again we shif and sum
          Gyr_rawY=TinyWireM.read()<<8|TinyWireM.read();
      TinyWireM.endTransmission(true);

      TinyWireM.beginTransmission(0x69);            //begin, Send the slave adress (in this case 68) 
      TinyWireM.write(0x43);                        //First adress of the Gyro data
      TinyWireM.endTransmission(false);
      TinyWireM.requestFrom(0x69,4);           //We ask for just 4 registers 
          Gyr_rawX2=TinyWireM.read()<<8|TinyWireM.read();     //Once again we shif and sum
          Gyr_rawY2=TinyWireM.read()<<8|TinyWireM.read();
      TinyWireM.endTransmission(true);

      
      /*---X---*/
      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8); 
      Gyro_raw_error_x2 = Gyro_raw_error_x2 + (Gyr_rawX2/32.8); 
      /*---Y---*/
      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
      Gyro_raw_error_y2 = Gyro_raw_error_y2 + (Gyr_rawY2/32.8);
            
      if(i==199)
      {
        Gyro_raw_error_x = Gyro_raw_error_x/200;
        Gyro_raw_error_y = Gyro_raw_error_y/200;
        gyro_error=1;

        Gyro_raw_error_x2 = Gyro_raw_error_x2/200;
        Gyro_raw_error_y2 = Gyro_raw_error_y2/200;
        gyro_error=1;
      }
    }
  }//end of gyro error calculation  
}//end of setup void


void loop() {
  timePrev = time;                        // the previous time is stored before the actual time read
  time = millis();                        // actual time read
  elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds
  
  //////////////////////////////////////Gyro read/////////////////////////////////////
    TinyWireM.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
    TinyWireM.write(0x43);                        //First adress of the Gyro data
    TinyWireM.endTransmission(false);
    TinyWireM.requestFrom(0x68,4);           //We ask for just 4 registers
    Gyr_rawX=TinyWireM.read()<<8|TinyWireM.read();     //Once again we shif and sum
    Gyr_rawY=TinyWireM.read()<<8|TinyWireM.read();
    
    TinyWireM.beginTransmission(true);
    TinyWireM.beginTransmission(0x69); 
    TinyWireM.write(0x43);    
    TinyWireM.endTransmission(false);
    TinyWireM.requestFrom(0x69,4);
    Gyr_rawX2=TinyWireM.read()<<8|TinyWireM.read();     //Once again we shif and sum
    Gyr_rawY2=TinyWireM.read()<<8|TinyWireM.read();
    TinyWireM.endTransmission(true);
    
    /*Now in order to obtain the gyro data in degrees/seconds we have to divide first
    the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/
    /*---X---*/
    Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; 
    Gyr_rawX2 = (Gyr_rawX2/32.8) - Gyro_raw_error_x2; 
    /*---Y---*/
    Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;
    Gyr_rawY2 = (Gyr_rawY2/32.8) - Gyro_raw_error_y2;  
      
    /*Now we integrate the raw value in degrees per seconds in order to obtain the angle
    * If you multiply degrees/seconds by seconds you obtain degrees */
    /*---X---*/
    Gyro_angle_x = Gyr_rawX*elapsedTime;
    Gyro_angle_x2 = Gyr_rawX2*elapsedTime;
    /*---X---*/
    Gyro_angle_y = Gyr_rawY*elapsedTime;
    Gyro_angle_y2 = Gyr_rawY2*elapsedTime;
  
  //////////////////////////////////////Acc read/////////////////////////////////////
  TinyWireM.beginTransmission(0x68); 
  TinyWireM.write(0x3B);
  TinyWireM.endTransmission();
  TinyWireM.requestFrom(0x68,6);
  while(TinyWireM.available() < 6);
  Acc_rawX=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; //each value needs two registres
  Acc_rawY=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
  Acc_rawZ=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; 
  TinyWireM.endTransmission(true);

  TinyWireM.beginTransmission(0x69); 
  TinyWireM.write(0x3B);
  TinyWireM.endTransmission();
  TinyWireM.requestFrom(0x69,6);
  Acc_rawX2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; //each value needs two registres
  Acc_rawY2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ;
  Acc_rawZ2=(TinyWireM.read()<<8|TinyWireM.read())/4096.0 ; 
  TinyWireM.endTransmission(true);

      
 /*Now in order to obtain the Acc angles we use euler formula with acceleration values
 after that we substract the error value found before*/  
 /*---X---*/
 Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;
 Acc_angle_x2 = (atan((Acc_rawY2)/sqrt(pow((Acc_rawX2),2) + pow((Acc_rawZ2),2)))*rad_to_deg) - Acc_angle_error_x2;
 /*---Y---*/
 Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_y;    
 Acc_angle_y2 = (atan(-1*(Acc_rawX2)/sqrt(pow((Acc_rawY2),2) + pow((Acc_rawZ2),2)))*rad_to_deg) - Acc_angle_error_y2;    


 //////////////////////////////////////Total angle and filter/////////////////////////////////////
 /*---X axis angle---*/
 Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;
 Total_angle_x2 = 0.98 *(Total_angle_x2 + Gyro_angle_x2) + 0.02*Acc_angle_x2;
 /*---Y axis angle---*/
 Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;
 Total_angle_y2 = 0.98 *(Total_angle_y2 + Gyro_angle_y2) + 0.02*Acc_angle_y2;
   

  Monitor.print("Xº1: ");
  Monitor.print(Total_angle_x);
  Monitor.print("   |   ");
  Monitor.print("Yº1: ");
  Monitor.print(Total_angle_y);
  Monitor.println(" ");
 delay(200);
 

  Monitor.print("Xº2: ");
  Monitor.print(Total_angle_x2);
  Monitor.print("   |   ");
  Monitor.print("Yº2: ");
  Monitor.print(Total_angle_y2);
  Monitor.println(" ");
  delay(200);
  Monitor.println("------------------------------------");


}

STEP 2: Knee angle calculation

To calculate the knee angle, I have referred to the following formulas provided on page 3 of this article. These calculations can be coded in different ways to find the knee angle. However, the code efficiency is different from one another. To decide the best code algorithm, I have written two codes and compared them based on execution time required or the space used.

knee

First try code:
  // Calculate knee angle
  int knee_angle;
  if( Total_angle_x>90 && Total_angle_x2>90 ){
    knee_angle = 180 - Total_angle_y + Total_angle_y2; 
  }
  if( Total_angle_x>90 && Total_angle_x2<=90 ){
    knee_angle = 360 - Total_angle_y - Total_angle_y2; 
  }
  if( Total_angle_x<=90 && Total_angle_x2>90 ){
    knee_angle = Total_angle_y + Total_angle_y2; 
  }
  if( Total_angle_x<=90 && Total_angle_x2<=90 ){
    knee_angle = 180 + Total_angle_y - Total_angle_y2; 
  }
First try execution time of algorithms:

firstTry_algorithims

First try code algorithm:

algorithm

Second try calculation code:
  // Calculate knee angle
int knee_angle;
  if( Total_angle_x>90 ){
    if ( Total_angle_x2>90 ){
      knee_angle = 180 - Total_angle_y + Total_angle_y2; 
    }
    else{
      knee_angle = 360 - Total_angle_y - Total_angle_y2; 
    }
  }
  else{
    if ( Total_angle_x2>90 ){
      knee_angle = Total_angle_y + Total_angle_y2; 
    }
    else{
      knee_angle = 180 + Total_angle_y - Total_angle_y2; 
    }
  }
Second try execution time of algorithms:

secondTry_algorithims

Results and Discussion:

Both codes are consist of a set of statements and don't contain loops, which means that they will always execute in the same time regardless of the input data size. Therefore, growth rates of both are linear O(1). Regarding the total time (total cost) required for each algorithm, second try code is less (3) than the first (9), which makes it better in terms of performance and memory space used. Nevertheless, both codes are executed in a short time since the difference is small.

Test result:

STEP 3: Vibration motor output.

I used a mini vibration motor as an alarm to inform the user to stop bending his/her leg more when it reachs 140 degree. It vibrates in a pattern of maximum PWM (255) and minimum (0) with a delay of 100 mSec. "knee_angle" is the measured range of movement based on the measrments by the Inertial Measurement Units (IMUs) - MPU6050 placed on the thigh and the shank.

if (knee_angle > 140)
   {  
      analogWrite(motorPin, 255);
      delay(100);
      analogWrite(motorPin, 0);
      delay(100);
   }

Video of the operating system: