10 DOF IMU read inaccurate when servo.h attached

Post here about your Arduino projects, get help - for Adafruit customers!

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
User avatar
Ollie_D
 
Posts: 2
Joined: Thu Sep 29, 2016 9:47 pm

10 DOF IMU read inaccurate when servo.h attached

Post by Ollie_D »

Hi,

In its barest form, I am trying to continuously control a linear actuator (6V servo) and read a gyroscope/accelerometer IMU (Adafruit 10 DOF IMU) simultaneously using my Arduino Mega 2560.

I am using the adafruit sensor library to read the sensors (gyroscope data and the accelerometer data). With these two readings, I am combining them using a filter to create an estimate of the orientation angle of the sensor (see attached code).

The sensor code works perfectly well when it is run stand-alone, but as soon as I attach the servo, via servo.attach(), my roll rate measurement (gyroscope reading around the x-axis) blows up and consequently corrupts my angle estimation (The rate goes from ~0.0001 rad/s to -6750000 rad/s). Oddly, however, the pitch rate measurement (reading around the y-axis) is undisturbed and yields correct angle estimate - where I have used exactly the same method to determine pitch and roll characteristics.

The problem persists when I use the MegaServo.h library. Only once I call servo.attach() does the problem present, even when no servo is connected. I have tried removing the motor shield and using directly connected to other pins on the arduino, as well as using a separate power supply to power the motor. In the loop, detaching the servo every call to reading the sensor fixes the issue at the expense of prohibiting the motor from moving at all.


As you can see, I am using a Timer2 interrupt to control the duration of the loop via setting a flag, to get regular sensor readings. I am also using Timer3 interrupt to run the motor forwards and backwards for a controlled duration, simultaneously with the sensor readings.

Could the interrupts be affecting the calibration functions in the setup loop, or are they only initialised outside of setup?

Many thanks for your help!

PS. I've also attached a shorter version of my code, demonstrating the issue I am having with the roll gyro reading. The error seems to occur in the 'CalibrateGyro()' function, in the roll-direction but not the pitch-direction. Oddly enough, every subsequent call to gyro_event.gyro.x outside of this function returns a correct value of the gyro reading.

The servo is:
https://s3-us-west-1.amazonaws.com/firg ... an2015.pdf

The IMU is:
https://learn.adafruit.com/adafruit-10- ... ting-it-up
The Sensor libraries used:
https://github.com/adafruit/Adafruit_Sensor

I've connected the IMU to my Arduino as per the above link - to the SCL pin (pin 21), SDA pin (pin 20) and then to ground and 5V.

The servo is connected to the Arduino via the Adafruit motor shield to pin 10.
The shield is:
https://learn.adafruit.com/adafruit-motor-shield

Code: Select all

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_10DOF.h>
#include <MegaServo.h>

#define First_servo_pin 10

/* Assign a unique ID to the sensors */
Adafruit_10DOF                dof   = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);

/* Create servo object */
MegaServo myservo;
bool extend = true;   // Flips direction of servo

double AngleZero[2];  // Accelerometer calibration zero point
double GyroZero[2];   // Gyroscope calibration zero point
double  rollAngle = 0; 
double  rollRate = 0;
double  pitchAngle = 0;
double  pitchRate = 0; 
uint8_t pos = 0;

volatile bool interval_passed = false;    // flag to read sensor
volatile bool write_motor =false;         // flag to run motor for an interval
unsigned long lastTime = 0;               // used to calculate looptime for angle est.
uint8_t skip = 0;

void initSensors()
{
  if(!accel.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!gyro.begin())
  {
    /* There was a problem detecting the L3GD20 ... check your connections */
    Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
    while(1);
  }
}

void setup() {
Serial.begin(115200);  
  /* Enable auto-ranging */
  gyro.enableAutoRange(true);
  
  /* Initialise the sensors */
  initSensors();        // checks that sensors are connected
  myservo.attach(First_servo_pin,990,2400); // adjust pwm frequency for servo
  calibrateAccel();     // zeros accelerometer
  calibrateGyro();      // zeros gyroscope

    /* Set up the interrupt signal */
  cli();    //stop interrupts
  
  TCCR3A = 0;// set entire TCCR3A register to 0
  TCCR3B = 0;// same for TCCR3B
  TCNT3  = 0;//initialize counter value to 0
  // set compare match register for 0.25hz increments
  OCR3A = 62500;// = (16*10^6) / (0.25*1024) - 1 (must be <65536)
  // turn on CTC mode
  TCCR3B |= (1 << WGM32);
  // Set CS32 and CS30 bits for 1024 prescaler
  TCCR3B |= (1 << CS32) | (1 << CS30);   
  // enable timer compare interrupt
  TIMSK3 |= (1 << OCIE3A);

  TCCR2A = 0;// set entire TCCR2A register to 0
  TCCR2B = 0;// same for TCCR2B
  TCNT2  = 0;//initialize counter value to 0
  // set compare match register for 100hz increments
  OCR2A = 155;// = (16*10^6) / (100*1024) - 1 (must be <256)
  // turn on CTC mode
  TCCR2A |= (1 << WGM21);
  // Set CS22 and CS20 and CS21 bits for 1024 prescaler
  TCCR2B |= (1 << CS22) | (1 << CS20) | (1 << CS21);   
  // enable timer compare interrupt
  TIMSK2 |= (1 << OCIE2A);

  
  sei();//allow interrupts
  
  lastTime = micros();

}

ISR(TIMER3_COMPA_vect)
{
  write_motor = true;
}

ISR(TIMER2_COMPA_vect)
{
  interval_passed = true;
} 

void loop() {
  unsigned long thisTime = micros();
  if (write_motor==true){
    if (extend == true){
      myservo.write(pos+70);
      extend = false;
    }
    else{
      myservo.write(pos-70);
      extend = true;
    }
    Serial.println(write_motor);
    write_motor=false;
  }
  if(!interval_passed){
    return;
  }
  // Get the IMU data
  sensors_event_t gyro_event;
  sensors_event_t accel_event;
  sensors_vec_t   orientation;
  gyro.getEvent(&gyro_event);
  accel.getEvent(&accel_event);
  dof.accelGetOrientation(&accel_event, &orientation);
  unsigned long dt = thisTime-lastTime;
  rollRate = gyro_event.gyro.x;
  pitchRate = gyro_event.gyro.y;
  rollAngle = orientation.roll-AngleZero[0];
  pitchAngle = orientation.pitch - AngleZero[1];
  lastTime = thisTime;
    
    if (skip++==50){
      Serial.print("dt:    "); Serial.print(dt);
      Serial.print("    RollAngle   "); Serial.print(rollAngle,4);
      Serial.print("    pitchAngle   "); Serial.print(pitchAngle,4);
      Serial.print("    rollRate     "); Serial.print(rollRate,4);
      Serial.print("    pitchRate    "); Serial.println(pitchRate,4);
      skip=0;
    }
  interval_passed=false;

}

void calibrateAccel()
{
  double roll=0;
  double pitch=0;
  for(uint8_t j=0; j<100; j++){
    sensors_event_t accel_event;
    sensors_vec_t   orientation;
    accel.getEvent(&accel_event);
    dof.accelGetOrientation(&accel_event, &orientation);
    roll += orientation.roll;
    pitch += orientation.pitch;
    delay(10);
  }
  AngleZero[0] = roll*0.01;
  AngleZero[1] = pitch*0.01;
 }

void calibrateGyro()
{
  double X,Y=0;
  for(uint8_t i=0; i<100;i++){
    sensors_event_t gyro_event;
    gyro.getEvent(&gyro_event);
    X += gyro_event.gyro.x;
    Y += gyro_event.gyro.y;
    delay(10);
  }
  GyroZero[0] = X*0.01;
  GyroZero[1] = Y*0.01;
}

User avatar
Ollie_D
 
Posts: 2
Joined: Thu Sep 29, 2016 9:47 pm

Re: 10 DOF IMU read inaccurate when servo.h attached

Post by Ollie_D »

If I attach the servo after I call the calibrate functions, the issue is resolved (using the servo.attach()/MegaServo.attach() function.
This is a 'good-enough' fix, although I was hoping to be able to actuate the motors before I calibrated the sensors - the sensor is tilted by the servo, so I wanted to set the sensor angle and zero the readings at that orientation.

If anyone has suggestions/ comments on this issue, I would be grateful to hear them.

Locked
Please be positive and constructive with your questions and comments.

Return to “Arduino”