I'm in 80% towards the target. Well, tired weeks. Now I have been in this step:
- The bot can be a self balancing robot (with no bluetooth).
- The bot can be controlled Arduino car (4 wheels with bluetooth).
Both are separated in coding. As you know that I'm a beginner in this coding, So far I'm just able to make both separated in two actions.
Here's the code:
/*
* ************************************************ INCL.LIBRARIES **************************************************** *
*/
#include <SoftwareSerial.h>
#include "I2Cdev.h"
#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
/*
* ****************************************** DECLARE ALL VARS AND DATA TYPES ***************************************** *
*/
SoftwareSerial bt(3,4);
char comm;
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
//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
VectorFloat gravity; //[x, y, z] gravity vector
float ypr[3]; //[yaw, pitch, roll] yaw/pitch/roll container and gravity vector
//set the value when the bot is perpendicular to ground using serial monitor.
double setpoint= 176;
double Kp = 21; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
volatile bool mpuInterrupt = false;
void dmpDataReady(){
mpuInterrupt = true;
}
/*
* ************************************************ VOID SETUP **************************************************** *
*/
void setup(){
Serial.begin(115200);
bt.begin(9600);
if(bt.available()){
pinMode(13,OUTPUT);//left motors forward
pinMode(12,OUTPUT);//left motors reverse
pinMode(11,OUTPUT);//right motors forward
pinMode(10,OUTPUT);//right motors reverse
}
else{
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
devStatus = mpu.dmpInitialize();
//supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1688);
//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();
//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(")"));
}
//Initialising
pinMode(13,OUTPUT);//left motors forward
pinMode(12,OUTPUT);//left motors reverse
pinMode(11,OUTPUT);//right motors forward
pinMode(10,OUTPUT);//right motors reverse
//Turning off both the motors
analogWrite(10,LOW);
analogWrite(11,LOW);
analogWrite(12,LOW);
analogWrite(13,LOW);
}
}
/*
* ************************************************ VOID LOOP **************************************************** *
*/
void loop() {
if(bt.available()){
comm = bt.read();
bt.println(comm);
delay(100);
if(comm == '1'){//move forward
digitalWrite(13,HIGH);
digitalWrite(12,LOW);
digitalWrite(11,HIGH);
digitalWrite(10,LOW);
}
else if(comm == '2'){//move reverse
digitalWrite(13,LOW);
digitalWrite(12,HIGH);
digitalWrite(11,LOW);
digitalWrite(10,HIGH);
}
else if(comm == '3'){//turn right
digitalWrite(13,LOW);
digitalWrite(12,LOW);
digitalWrite(11,HIGH);
digitalWrite(10,LOW);
}
else if(comm == '4'){//turn left
digitalWrite(13,LOW);
digitalWrite(12,HIGH);
digitalWrite(11,LOW);
digitalWrite(10,LOW);
}
else if(comm == '5'){//STOP (Motors stop)
digitalWrite(13,LOW);
digitalWrite(12,LOW);
digitalWrite(11,LOW);
digitalWrite(10,LOW);
}
}
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//If bt not available or car in perpendicular position, run this follows
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();
//Print the value of Input and Output on serial monitor to check how it is working.
Serial.print(input); Serial.print(" =>"); Serial.println(output);
if (input>150 && input<200){//If the Bot is falling
if (output>0) //Falling towards front
Forward(); //Rotate the wheels forward
else if (output<0) //Falling towards back
Reverse(); //Rotate the wheels backward
}
else //If Bot not falling
Stop(); //Hold the wheels still
}
//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(fifoBuffer, 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); //get value for q
mpu.dmpGetGravity(&gravity, &q); //get value for gravity
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr
input = ypr[1] * 180/M_PI + 180;
}
}
/*
* ************************************* CALL THESE FOLLOWS WHEN MPU6050 ACTIVATED ******************************************
*/
void Forward(){
analogWrite(13,output);
analogWrite(12,0);
analogWrite(11,output);
analogWrite(10,0);
Serial.print("F"); //Debugging information
}
void Reverse(){
analogWrite(13,0);
analogWrite(12,output*-1);
analogWrite(11,0);
analogWrite(10,output*-1);
Serial.print("R");
}
void Stop(){
analogWrite(13,0);
analogWrite(12,0);
analogWrite(11,0);
analogWrite(10,0);
Serial.print("S");
}
Then, here's the bot looks like:
(note: I place the mpu6050 below the bot (among the wheels). The bot can stand (self balancing) using 2 wheels = when not using the bluetooth or if bt serial is not Available.
This is the real diagram circuit:
(really sorry for bad drawing).
NOTE:
In void_setup you can see that I use if to make it separated as in below:
void setup(){
Serial.begin(115200);
bt.begin(9600);
if(bt.available()){
pinMode(13,OUTPUT);//left motors forward
pinMode(12,OUTPUT);//left motors reverse
pinMode(11,OUTPUT);//right motors forward
pinMode(10,OUTPUT);//right motors reverse
}
else{
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
//verify connection
Serial.println(
I do the same as in void loop
CURRENT TRY:
NOW, I'M trying to code how to combine both = the bot can stand (self balancing), can be controlled by bluetooth and can be controlled by bluetooth when it becomes a 4wd arduino car.
I'm a web developer and C++ is new for me 
(now I begin loving it).