here is my code, part1:
#include <Encoder.h>
#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
char blueToothVal; //value sent over via bluetooth
char lastValue; //stores last state of device (on/off)
#endif
/* ===========================================================
* ========= MPU Definitions ==============================
* ===========================================================
*/
//Define MPU 6050 Variables
MPU6050 mpu;
int t1=0;
// MPU control/status vars
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
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
VectorInt16 aaOffset; // [x, y, z] accel sensor measurements offset
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float yprOffset[] = { 0, 0.15, 0 }; // [yaw, pitch, roll] offset yaw/pitch/roll container and gravity vector
float gyroOffset[] = {0, 0, 0}; //[x,y,z] offsets look for the axes on the MPU
int16_t gyro[3]; //[gx ,gy, gz] deg/sec
// INTERRUPT DETECTION ROUTINE
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
#define LED_PIN 13
float z=0.4;
bool blinkState = false;
/* ===========================================================
* ========= Encoder Definitions ==========================
* ===========================================================
*/
//State the Pins of the Encoder using Interrupt pins
Encoder myEnc(9, 10);
// Right wheel drive (Encoder working on Right Motor)
Servo victorRight;
Servo victorLeft;
//Intial Vars:
//Encoder Related var:
long POS1 = 0;
long POS2 = 0;
long POS3 = 0;
long POS4 = 0;
long POS5 = 0;
long oldPosition = -999;
long npo;
long newPosition = 0;
int xT = 0;
int sT = 0;
int dT = 0;
/*=========================================================
* ================== State Space ========================
* =========================================================
*/
float K[2][6] = { // Velocity via Encoders Only
// { -27, -2.707, -0.5, -100, -30, -4}, // 3-> -34.7573
//{ -27, -2.707, -0.5, -100, 30, 4 }
//{ -37, -1.707, -2, -10, -7, -2}, //good
//{ -37, -1.707, -2, -10, 7, 2 }
{ -47, -2.707, -1, -5, -30, -4}, //good
{ -47, -2.707, -1, -5, 30, 4 }
// { -50, -0.707, -1, -15, -15.2361, -2.3324}, // 3-> -34.7573
// { -50, -0.707, -1, -15, 15.2361, 2.3324 }
};
// Motor Voltage From State Space vars:
float Vr = 0;
float Vl = 0;
//State Reading Vector: [ theta, x, theta_dot, x_dot, psi, psi_dot]'
volatile float State_Readings[] = {0,0,0,0,0,0};
//Desired Velocity in Meter / Sec
float D_Velocity = 0;
//Desired Steer Angle in radians
float D_Steer = 0;
float D_Angle = 0;
void setup()
{
int npo=0;
Serial.begin(115200);
// Serial.begin(9600);
// Attach Pin to right motor
victorRight.attach(8);
victorLeft.attach(6);
pinMode(12,OUTPUT);
int t1=0;
// Mpu communications protocol
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//Mpu Setup
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
// These Offset tested for each MPU, IMPORTANT STEP
mpu.setXGyroOffset(178);
mpu.setYGyroOffset(19);
mpu.setZGyroOffset(32);
mpu.setXAccelOffset(1022);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(1310);
// 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 our 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.dmpGetFIFOPacketSize();
}
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(")"));
}
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}