#include "MPU9250.h"
#include "math.h"
MPU9250 mpu;
#include<Servo.h>
Servo servo1;
//intialising the servos
Servo servo2;
Servo servo3;
float servo1angle = 0;
float servo2angle = 0;
float servo3angle = 0;
double pre_ts = 0;
float cQ[4] = { 0, 0, 0, 0 }; //the current unit quaternions
float dQ[4] = { 1, 0, 0, 0 }; //the desired quaternion values
float eQ[4] = { 0, 0, 0, 0 }; //the error quaternion values
float eQI[4] = { 0, 0, 0, 0 };
int
sgnp (float a)
{
if (a >= 0)
return 1;
return -1;
}
void
setup ()
{
Serial.begin (57400);
Wire.begin ();
delay (100);
servo1.attach (9);
servo2.attach (5);
servo3.attach (6);
delay (100);
if (!mpu.setup (0x68))
{
// change to your own address
while (1)
{
Serial.
println
("MPU connection failed. Please check your connection with
`connection_check` example.");
delay (500);
}
}
//
mpu.setFilterIterations(10);
mpu.setMagneticDeclination (0.37);
// calibrate anytime you want to
Serial.println ("Accel Gyro calibration will start in 5sec.");
Serial.println ("Please leave the device still on the flat plane.
");
mpu.verbose (true);
delay (3000);
mpu.calibrateAccelGyro ();
Serial.println ("Mag calibration will start in 5sec.");
Serial.println ("Please Wave device in a figure eight until done.
");
delay (3000);
mpu.calibrateMag ();
delay (4000);
print_calibration ();
mpu.verbose (false);
pre_ts = millis ();
}
void
loop ()
{
if (mpu.update ())
{
print_roll_pitch_yaw ();
cQ[1] = mpu.getQuaternionX ();
cQ[2] = mpu.getQuaternionY ();
cQ[3] = mpu.getQuaternionZ ();
cQ[0] = mpu.getQuaternionW ();
Serial.print (pre_ts, 2);
quaternioncontroller (millis () - pre_ts);
pre_ts = millis ();
}
else if (millis () - pre_ts > 40) // to stop the servo motion if
connection with IMU is lost
{
servo1.writeMicroseconds (1500);
servo2.writeMicroseconds (1500);
servo3.writeMicroseconds (1500);
}
}
void
print_roll_pitch_yaw ()
{
Serial.print ("Yaw, Pitch, Roll: ");
Serial.print (mpu.getYaw (), 2);
Serial.print (", ");
Serial.print (mpu.getPitch (), 2);
Serial.print (", ");
Serial.println (mpu.getRoll (), 2);
}
void
quaternioncontroller (double dt)
{
eQ[0] = dQ[3] * cQ[3] + dQ[2] * cQ[2] + dQ[1]
cQ[0]; //calculatin unit error quaternions
eQ[1] = dQ[3] * cQ[2] - dQ[1] * cQ[0] - dQ[2]
cQ[1];
eQ[2] = dQ[1] * cQ[3] - dQ[0] * cQ[2] - dQ[2]
cQ[1];
eQ[3] = dQ[2] * cQ[1] - dQ[3] * cQ[0] - dQ[0]
cQ[2];
* cQ[1] - dQ[0] *
* cQ[3] - dQ[0] *
* cQ[0] - dQ[3] *
* cQ[3] - dQ[1] *
if (eQ[1] > 0.02 || eQ[1] < -0.02)
{
eQ[1] = -1 * (1500) * sgnp (eQ[0]) * eQ[1];
eQI[1] = eQI[1] + eQ[1];
servo1angle = 1500 - eQ[1] - (eQI[1] / 60);
servo1.writeMicroseconds (servo1angle);
}
else
{
servo1.writeMicroseconds (1523); //driving individual axes
eQI[1] = 0;
}
if (eQ[2] > 0.02 || eQ[2] < -0.02)
{
eQ[2] = -1 * (1200) * sgnp (eQ[0]) * eQ[2];
eQI[2] = eQI[2] + eQ[2];
servo2angle = 1500 - eQ[2] - (eQI[2] / 60);
servo2.writeMicroseconds (servo2angle);
}
}
else
{
servo2.writeMicroseconds (1460);
eQI[2] = 0;
}
if (eQ[3] > 0.02 || eQ[3] < -0.02)
{
eQ[3] = -1 * (500) * sgnp (eQ[0]) * eQ[3];
eQI[3] = eQI[3] + eQ[3];
servo3angle = 1500 - eQ[3] - (eQI[3] / 40);
servo3.writeMicroseconds (servo3angle);
}
else
{
servo3.writeMicroseconds (1523);
eQI[3] = 0;
}