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;
}