Hi everyone! I am new member of this community and I have finished one of my project Self balancing robot following the code from this man (many thanks)
I didn't change it much, just change the PID and pins used.It works. The problems is when I tried to connect my HC05 module into this schematic (rx/tx:0/1) with the control code from this man
my robot didn't work right. I want to ask someone who has did this project for the combination of balancing and remote code in one.
My schematic:
I uploaded the codes below.
#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include <SimpleKalmanFilter.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LOG_INPUT 0//Hiển thị trên serial plotter
#define MANUAL_TUNING 0//Hiệu chỉnh bằng tay
#define LOG_PID_CONSTANTS 0 //Ghi lại các giá trị hiệu chỉnh
#define MOVE_BACK_FORTH 0//Điều chỉnh setpoint để kéo robot theo hướng ngược lại. Chỉ số movingAngleOffset nên để nhỏ cỡ 0.1 - 0.2. Để lớn quá thì robot dễ mất ổn định
#define MIN_ABS_SPEED 5//Mức tốc độ tối thiểu
//RC
SoftwareSerial mySerial(0, 1);
String data;
char key;
//MPU
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
//PID
#define original_setpoint_offsets -1.7//Mặc định với cấu hình hiện tại (-2.3 -2.2)
#if MANUAL_TUNING
double kp , ki, kd;
double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 0 + original_setpoint_offsets;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.3;//bù vừa phải
double input, output;
int moveState=0;
//Tham số PID: 19 160 1.7
#define Kp 19
#define Ki 170
#define Kd 1.5
#if MANUAL_TUNING
PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
#endif
//MOTOR CONTROLLER
#define ENA 3
#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7
#define ENB 9
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);
//timers
long time1Hz = 0;
long time5Hz = 0;
long timeRC=0;
long timePID=0;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
/////////////////////////////////////////////// cac ham function //////////////////////////////////////////////////////
void balance();
void loopAt1Hz();
void loopAt5Hz();
void moveBackForth();
void setPIDTuningValues();
void readPIDTuningValues();
void RC();
void loop_RC();
void forward();
void reverse();
void left();
void right();
void stoprobot();
void boost();
void spinright();
void spinleft();
////////////////////////////////////////////// setup //////////////////////////////////////////////////////////
void setup()
{
Serial.println(ypr[1] * 180/M_PI);
// 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
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
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
mpu.setXGyroOffset(39);
mpu.setYGyroOffset(14);
mpu.setZGyroOffset(6);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// 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(5);
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(")"));
}
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
//pinMode(EN1, OUTPUT);
//pinMode(EN2, OUTPUT);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
//analogWrite(EN1,63);
//analogWrite(EN2,63);
mySerial.begin(115200);
}
/////////////////////////////////////////////// loop //////////////////////////////////////////////////////////
void loop()
{
balance();
loop_RC();
}
//loop_RC
void loop_RC()
{
//unsigned long currentMillis_RC = millis();
if (mySerial.available() < 0)
{
Serial.println("No Bluetooth Data ");
}
else
{
balance();
while (mySerial.available())
{
{
data= mySerial.readStringUntil('\n');
//Serial.print(str);
}
key = (data.toInt());
switch (key)
{
//Tay cầm bên trái
case 'F':
Serial.println("Forward");
forward();
break;
case 'B':
Serial.println("Reverse");
reverse();
break;
case 'L':
Serial.println("Left");
left();
break;
//Tay cầm bên phải
case 'R':
Serial.println("Right");
right();
break;
case 'T':
Serial.println("Right");
boost();
break;
case 'C':
Serial.println("Right");
spinright();
break;
case 'S':
Serial.println("Right");
spinleft();
break;
case 'X':
Serial.println("Right");
//balance();
stoprobot();
break;
//case 'A':
//Serial.println("Start");
//balance();
//break;
}
}
}
}
//balance
//////////////////////////////
void balance()
{
// 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(output, MIN_ABS_SPEED);
unsigned long currentMillis = millis();
if (currentMillis - time1Hz >= 100)
{
loopAt1Hz();
time1Hz = currentMillis;
}
if (currentMillis - time5Hz >= 300)
{
loopAt5Hz();
time5Hz = currentMillis;
}
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
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);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#if LOG_INPUT
Serial.print("Yaw:");
Serial.println(ypr[0] * 180/M_PI);
Serial.print("Pitch:");
Serial.println(ypr[1] * 180/M_PI);
Serial.print("Roll:");
Serial.println(ypr[2] * 180/M_PI);
Serial.print("setpoint:");
Serial.println(setpoint);
Serial.println();
#endif
input = ypr[1] * 180/M_PI;
}
}
////////////////////////
//movement
void forward()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, 130);
analogWrite(ENB, 130);
}
void reverse()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENA, 130);
analogWrite(ENB, 130);
}
void left()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, 130);
}
void right()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENB, 130);
}
void stoprobot()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
analogWrite(ENA, 0);
analogWrite(ENA, 0);
}
void boost()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, 200);
analogWrite(ENB, 200);
}
void spinright()
{
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, 80);
analogWrite(ENA, 80);
}
void spinleft()
{
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, 80);
analogWrite(ENA, 80);
}