#include

<SoftwareSerial.h>
#include <PID_v1.h>
#include
<LMotorController.h>
#include <I2Cdev.h>
#include <MPU-
6050_6Axis_MotionApps2
0.h>
MPU-6050 mpu;
bool dmpReady = false; //
set true if DMP init was
successful
uint8_t mpuIntStatus; //
holds actual interrupt
status byte from MPU
uint8_t devStatus; // return
status after each device
operation (0 =
success, !0 = error)
uint16_t packetSize; //
expected DMP packet size
(default is 42 bytes)
uint16_t fifoCount; // count
of all bytes currently in
FIFO
uint8_t fifoBuffer[64]; //
FIFO storage buffer
volatile bool mpuInterrupt
= false; // indicates
whether MPU interrupt pin
has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
Quaternion q; // [w, x, y, z]
quaternion container
VectorFloat gravity; // [x, y,
z] gravity vector
float ypr[3]; // [yaw, pitch,
roll] yaw/pitch/roll
container and gravity
vector
double input, output;
double Kp = 42.8;// PID
Constants
double Ki = 374;
double Kd = 2.5;
PID pid(&input, &output,
&setpoint, Kp, Ki, Kd,
DIRECT);
double originalSetpoint =
185.68;
double setpoint =
originalSetpoint;
double motorSpeedA=
0.45;
double motorSpeedB=
0.55;
int ENA = 10;
int IN1 = 9;
int IN2 = 8;
int IN3 = 7;
int IN4 = 6;
int ENB = 5;
LMotorController
motorController(ENA, IN1,
IN2,
ENB, IN3, IN4,
motorSpeedA,
motorSpeedB);
#if
I2CDEV_IMPLEMENTATI
ON ==
I2CDEV_ARDUINO_WIR
E
Wire.begin();
TWBR = 24; // 400kHz I2C
clock (200kHz if CPU is
8MHz)
#elif
I2CDEV_IMPLEMENTATI
ON ==
I2CDEV_BUILTIN_FAST
WIRE
Fastwire::setup(400, true);
#endif
Serial.println(F("Initializing
I2C devices..."));
mpu.initialize();
Serial.println(F("Testing
device connections..."));
Serial.println(mpu.testCon
nection() ? F("MPU-6050
connection
successful") : F("MPU-
6050 connection failed"));
Serial.println(F("Initializing
DMP..."));
devStatus =
mpu.dmpInitialize();
// make sure it worked
(returns 0 if so)
if (devStatus == 0)
{
// turn on the DMP, now
that it's
ready
Serial.println(F("Enabling
DMP..."));
mpu.setDMPEnabled(true)
;
// enable Arduino interrupt
detection
Serial.println(F("Enabling
interrupt detection
(Arduino external interrupt
0)..."));
attachInterrupt(0,
dmpDataReady, RISING);
mpuIntStatus =
mpu.getIntStatus();
// set DMP Ready flag so
the
main loop() function
knows it's okay to use it
Serial.println(F("DMP
ready! Waiting for first
interrupt..."));
dmpReady = true;
// get expected DMP
packet size for later
comparison
packetSize =
mpu.dmpGetFIFOPacketS
ize();
//setup PID
pid.SetMode(AUTOMATIC
);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255,
255);
}
else
{
// ERROR!
// 1 = initial memory load
failed
// 2 = DMP configuration
updates failed
// (if it's going to break,
usually the code will be 1)
Serial.print(F("DMP
Initialization failed (code
"));
Serial.print(devStatus);
Serial.println(F(")"));
}
mpu.setXGyroOffset(17);
mpu.setYGyroOffset(78);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(751);
mpu.setXAccelOffset(136
9);
mpu.setYAccelOffset(-48);
// if programming failed,
don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or
extra packet(s) available
while (!mpuInterrupt &&
fifoCount < packetSize)
{
//no mpu data - performing
PID calculations and
output to motors
pid.Compute();
motorController.move(outp
ut, MIN_ABS_SPEED);
}
// reset interrupt flag and
get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus =
mpu.getIntStatus();
// get current FIFO count
fifoCount =
mpu.getFIFOCount();
// check for overflow (this
should never happen
unless our code is too
inefficient)
if ((mpuIntStatus & 0x10) ||
fifoCount == 1024)
{
// reset so we can
continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO
overflow!"));
// otherwise, check for
DMP data ready interrupt
(this should happen
frequently)
}
else if (mpuIntStatus &
0x02)
{
// wait for correct available
data length, should be a
very short wait
while (fifoCount <
packetSize) fifoCount =
mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuff
er, packetSize);
// track FIFO count here in
case there is > 1 packet
available
// (this lets us immediately
read more without waiting
for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&
q, fifoBuffer);
mpu.dmpGetGravity(&gra
vity, &q);
mpu.dmpGetYawPitchRoll
(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI
+ 180;
const int rxpin = 11;
const int txpin = 3;
SoftwareSerial blue(rxpin,
txpin);
char var = 0;
int moveState = 0;
Serial.begin(115200);
blue.begin(38400);
blue.setTimeout(10);
if(Serial.available()>0)
{
char var = Serial.read();
delay(10);
if(var =='F')
{
setpoint = originalSetpoint
- movingAngleOffset;
digitalWrite (IN1, HIGH);
digitalWrite (IN2, LOW);
digitalWrite (IN3, HIGH);
digitalWrite (IN4, LOW);
analogWrite (ENA,
abs(MIN_ABS_SPEED));
analogWrite (ENB,
abs(MIN_ABS_SPEED));
delay(1000);//forward
}
else if(var =='B')
{
setpoint = originalSetpoint
+ movingAngleOffset;
digitalWrite (IN1, LOW);
digitalWrite (IN2, HIGH);
digitalWrite (IN3, LOW);
digitalWrite (IN4, HIGH);
analogWrite (ENA,
abs(MIN_ABS_SPEED));
analogWrite (ENB,
abs(MIN_ABS_SPEED));
delay(1000);//backward
}
}