With the same code I asked in the previous question. I need to find out how bluetooth can control self-balancing robot in a code lines.
Specification:
- Arduino UNO
- L298N Module Driver (4 pinouts with enA and enB disabled - covered with jumper)
- TT Gear Motor (Yellow)
- Bluetooth HC-05
- MPU6050
Here's the source code (current self-balancing robot has its balance enough):
#include "I2Cdev.h"
#include <Wire.h>
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>
#include <digitalIOPerformance.h> //library for faster pin R/W
//#include <Ultrasonic.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define d_speed 1.5
#define d_dir 3
#define IN1 11
#define IN2 10
#define IN3 9
#define IN4 3
char content = 'P';
int MotorAspeed, MotorBspeed;
float MOTORSLACK_A = 26; // Compensate for motor slack range (low PWM values which result in no motor engagement)
float MOTORSLACK_B = 26;
#define BALANCE_PID_MIN -190 // Define PID limits to match PWM max in reverse and foward
#define BALANCE_PID_MAX 190
MPU6050 mpu;
const int rxpin = 6; //Bluetooth serial stuff
const int txpin = 5;
SoftwareSerial blue(rxpin, txpin);
//Ultrasonic ultrasonic(A0, A1);
//int distance;
// MPU control/status vars
bool dmpReady = true; // 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
/*********Tune these 4 values for your BOT*********/
double setpoint = 177.64; //set the value when the bot is perpendicular to ground using serial monitor.
double originalSetpoint;
// Read the project documentation on circuitdigest.com to learn how to set these values
#define Kp 21 //Set this first
#define Kd 1.01 //Set this secound
#define Ki 40 //Finally set this
#define RKp 21 //Set this first
#define RKd 1.01 //Set this secound
#define RKi 40 //Finally set this
/******End of values setting*********/
double ysetpoint;
double yoriginalSetpoint;
double input, yinput, youtput, output, Buffer[3];
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
PID rot(&yinput, &youtput, &ysetpoint, RKp, RKi, RKd, DIRECT);
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup() {
// 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
Serial.begin(115200);
blue.begin(9600);
blue.setTimeout(40);
init_imu(); //initialiser le MPU6050
initmot(); //initialiser les moteurs
originalSetpoint = 177.64; //consigne
yoriginalSetpoint = 0.1;
setpoint = originalSetpoint ;
ysetpoint = yoriginalSetpoint ;
}
void loop() {
getvalues();
Bt_control();
printval();
}
void init_imu() {
// initialize device
Serial.println(F("Initializing I2C devices..."));
Wire.begin();
TWBR = 24;
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(120);
mpu.setYGyroOffset(46);
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); // -255, 255
rot.SetMode(AUTOMATIC);
rot.SetSampleTime(10);
rot.SetOutputLimits(-20, 20); // -20, 20
}
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(")"));
}
}
void getvalues() {
// 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) {
new_pid();
}
// 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;
yinput = ypr[0] * 180 / M_PI;
}
}
void new_pid() {
//Compute error
pid.Compute();
rot.Compute();
// Convert PID output to motor control
MotorAspeed = compensate_slack(youtput, output, 140);
MotorBspeed = compensate_slack(youtput, output, 140);
motorspeed(MotorAspeed, MotorBspeed); //change speed
}
//Fast digitalWrite is implemented
void Bt_control() {
if (blue.available()) {
content = blue.read();
if (content == 'F')
setpoint = originalSetpoint - d_speed;//Serial.println(setpoint);} //forward
else if (content == 'B')
setpoint = originalSetpoint + d_speed;//Serial.println(setpoint);} //backward
else if (content == 'L')
ysetpoint = constrain((ysetpoint + yoriginalSetpoint - d_dir), -180, 180); //Serial.println(ysetpoint);} //left
else if (content == 'R')
ysetpoint = constrain(ysetpoint + yoriginalSetpoint + d_dir, -180, 180); //Serial.println(ysetpoint);} //right
else if (content == 'S') {
setpoint = originalSetpoint;
}
}
else { content = 'P';}
}
void initmot() {
//Initialise the Motor outpu pins
pinMode (IN4, OUTPUT);
pinMode (IN3, OUTPUT);
pinMode (IN2, OUTPUT);
pinMode (IN1, OUTPUT);
//By default turn off both the motor
analogWrite(IN4, LOW);
analogWrite(IN3, LOW);
analogWrite(IN2, LOW);
analogWrite(IN1, LOW);
}
double compensate_slack(double yOutput, double Output, bool A) {
// Compensate for DC motor non-linear "dead" zone around 0 where small values don't result in movement
//yOutput is for left,right control
if (A)
{
if (Output >= 0)
Output = Output + MOTORSLACK_A - yOutput;
if (Output < 0)
Output = Output - MOTORSLACK_A - yOutput;
}
else
{
if (Output >= 0)
Output = Output + MOTORSLACK_B + yOutput;
if (Output < 0)
Output = Output - MOTORSLACK_B + yOutput;
}
Output = constrain(Output, BALANCE_PID_MIN, BALANCE_PID_MAX);
return Output;
}
void motorspeed(int MotorAspeed, int MotorBspeed) {
// Motor A control
if (MotorAspeed >= 0) {
analogWrite(IN1, abs(MotorAspeed));
digitalWrite(IN2, LOW);
}
else {
digitalWrite(IN1, LOW);
analogWrite(IN2, abs(MotorAspeed));
}
// Motor B control
if (MotorBspeed >= 0) {
analogWrite(IN3, abs(MotorBspeed));
digitalWrite(IN4, LOW);
}
else {
digitalWrite(IN3, LOW);
analogWrite(IN4, abs(MotorBspeed));
}
}
void printval()
{
Serial.print(yinput); Serial.print("\t");
Serial.print(yoriginalSetpoint); Serial.print("\t");
Serial.print(ysetpoint); Serial.print("\t");
Serial.print(youtput); Serial.print("\t"); Serial.print("\t");
Serial.print(input); Serial.print("\t");
Serial.print(originalSetpoint); Serial.print("\t");
Serial.print(setpoint); Serial.print("\t");
Serial.print(output); Serial.print("\t"); Serial.print("\t");
Serial.print(MotorAspeed); Serial.print("\t");
Serial.print(MotorBspeed); Serial.print("\t"); Serial.print(content); Serial.println("\t");
}
From the source code, I see a part of bluetooth code lines as follows:
void Bt_control() {
if (blue.available()) {
content = blue.read();
if (content == 'F')
setpoint = originalSetpoint - d_speed;//Serial.println(setpoint);} //forward
else if (content == 'B')
setpoint = originalSetpoint + d_speed;//Serial.println(setpoint);} //backward
else if (content == 'L')
ysetpoint = constrain((ysetpoint + yoriginalSetpoint - d_dir), -180, 180); //Serial.println(ysetpoint);} //left
else if (content == 'R')
ysetpoint = constrain(ysetpoint + yoriginalSetpoint + d_dir, -180, 180); //Serial.println(ysetpoint);} //right
else if (content == 'S') {
setpoint = originalSetpoint;
}
}
else { content = 'P';}
}
As a beginner in Arduino, I can see that setPoint = 177.64 - d_speed (where d_speed has value "1.5") make the robot goes forward.
I can see that setPoint 177.64 is the nearest real value from Serial Monitor.
Using the custom MIT App which is appointed by the owner of the code, The robot can not be controlled.
What I want to know ... "Is the bluetooth code lines proper to push the robot forward and backward."
NOTE:
This following config has been in its correct pins:
const int rxpin = 6; //Bluetooth serial stuff
const int txpin = 5;
SoftwareSerial blue(rxpin, txpin);
NB: Circuit
