Hello everyone! I am currently working in the code of my self balancing robot and I have some problems. The robot itself balances until it tilts too much to one side and when that happens the robot starts accelerating until it hits something. I do not know what is exactly the problem.
At the bottom of the post I attached a video of it
Code:
//////////////////////////////////////////////////////////////
//// status LED stuff
#define statusPin 13
//#define statusPin 2 // for ESP8266-12
// Flashes the led without using the delay() function.
// The period of each cycle is flashDelay milliseconds
// flashSpeed determines the speed of each flash
// flashTimes determines the number of times to flash in a cycle
// Need to call this function inside a loop in order to flash
// in order to notice a pause between each cycle
// flashDelay should be greater than flashSpeed*2*flashTimes
// e.g. flashActivityLed(1000, 100, 1);
void flashActivityLed(int flashDelay=3000, int flashSpeed=150, int flashTimes=2) {
static unsigned long nextLedActivity = 0;
static int t = 0;
if ((millis() > nextLedActivity)) {
if (t == 0) {
digitalWrite(statusPin, HIGH); // turn on LED
} else {
digitalWrite(statusPin, !digitalRead(statusPin)); // toggle LED
}
if (t < flashTimes*2-1) {
nextLedActivity = millis() + flashSpeed;
t++;
} else {
nextLedActivity = millis() + flashDelay - (flashSpeed*2*flashTimes);
t = 0;
}
}
}
//////////////////////////////////////////////////////////////
// PID stuff
// PID stands for proportional-integral-derivative
// A PID controller uses a feedback loop mechanism to control a system
// to maintain a steady state.
#include <PID_v1.h>
double setpoint = 184; //set the value when the robot is perpendicular to ground using serial monitor.
// this is good! works
//double Kp = 17; //Set this first
//double Kd = 0.7; //Set this secound
//double Ki = 120; //Finally set this
// this is good! works
double Kp = 23; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this
/******End of values setting*********/
double pidInput, pidOutput;
PID pid(&pidInput, &pidOutput, &setpoint, Kp, Ki, Kd, DIRECT);
void initPID() {
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
//////////////////////////////////////////////////////////////
// MPU6050 stuff
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
//#define USE_INTERRUPT // define this to use interrupts
// the INT pin must be connected if using interrupts
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
//#define INTERRUPT_PIN 15 // use pin 15 on ESP8266
volatile bool mpuInterrupted = false; // indicates whether MPU interrupt pin has gone high
//// Interrupt detection routine
// When the MPU has data, it will interrupt the Arduino causing it
// to execute this routine automatically
void dmpDataReady() {
mpuInterrupted = true;
}
//// initialize and calibrate the MPU
//// return true if successful
bool initMPU() {
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize device
Serial.println("Initializing I2C devices...");
mpu.initialize();
pinMode(INTERRUPT_PIN, pidInput);
// verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
Serial.print("Calibrating DMP...");
// Don't need to calibrate the zero values.
// We always want the robot to be upright at the setpoint value
// So it is important to set the setpoint value for when the robot is upright
// mpu.CalibrateAccel(6);
// mpu.CalibrateGyro(6);
// mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print("Enabling interrupt detection (Arduino external interrupt #");
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.print(" on pin ");
Serial.print(INTERRUPT_PIN);
Serial.println(")...");
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println("DMP ready! Waiting for first interrupt...");
return true;
} 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("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
while(1); // stop here if init failed
return false;
}
}
// Check if there's data from the MPU
// and if there's data then
// get and return the gyro x, y, z axis data
// This routine must be called very often
// Otherwise the FIFO will overflow
void checkMPU(float xyz[3]) {
// MPU control/status vars
static uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
static uint16_t fifoCount; // count of all bytes currently in FIFO
static uint8_t fifoBuffer[64]; // FIFO storage buffer
static uint16_t packetSize = mpu.dmpGetFIFOPacketSize(); // expected DMP packet size for later comparison (default is 42 bytes)
// orientation/motion vars
static Quaternion q; // [w, x, y, z] quaternion container
static VectorFloat gravity; // [x, y, z] gravity vector
static float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
#ifdef USE_INTERRUPT
// check if there's data in the fifo
if (!mpuInterrupted && fifoCount < packetSize) {
return; // no data yet, so return
}
#endif
// reset interrupt flag and get INT_STATUS byte
mpuInterrupted = 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
// fifoCount = mpu.getFIFOCount(); // will be zero after reset no need to ask
Serial.println("FIFO overflow!");
}
// otherwise, check for DMP data ready interrupt (this should happen frequently)
else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// 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);
// get and return the gyro x, y, z axis data
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
xyz[0] = ypr[2] * 180/M_PI; // roll(x axis)
xyz[1] = ypr[1] * 180/M_PI; // pitch(y axis)
xyz[2] = ypr[0] * 180/M_PI; // yaw(z axis)
}
}
// for testing only
void testGyro() {
static float xyz[3];
do {
checkMPU(xyz);
Serial.print("x = ");
Serial.print(xyz[0]);
Serial.print(" y = ");
Serial.print(xyz[1]);
Serial.print(" z = ");
Serial.print(xyz[2]);
pidInput = xyz[0] + 180; // get and adjust X-axis value
pid.Compute(); // compute the new PID correction value
Serial.print(" ");
Serial.print(pidInput); Serial.print(" => "); Serial.print(pidOutput);
Serial.println();
} while(1);
}
/////////////////////////////////////////////////////////////
// Motor stuff
// Need to put in the correct values for the analogWrite commands
// in the goForward() and goBackward() functions so that the
// robot does what the functions are intended to do
int in1Pin = 10; // right motor
int in2Pin = 11; // right motor
int in3Pin = 6; // left motor
int in4Pin = 5; // left motor
void initMotor() {
// controls for right motor 1
pinMode(in1Pin, OUTPUT);
pinMode(in2Pin, OUTPUT);
// controls for left motor 2
pinMode(in3Pin, OUTPUT);
pinMode(in4Pin, OUTPUT);
//By default turn off both motors
analogWrite(in1Pin, 0);
analogWrite(in2Pin, 0);
analogWrite(in3Pin, 0);
analogWrite(in4Pin, 0);
}
void stopMotor(int delayTime = 0) {
analogWrite(in1Pin, 0);
analogWrite(in2Pin, 0);
analogWrite(in3Pin, 0);
analogWrite(in4Pin, 0);
if (delayTime > 0) {
delay(delayTime);
}
}
void goForward(int delayTime = 0, int motorSpeed = 255) {
// change the 0 and motorSpeed for each pair so that the robot will go forward
motorSpeed = abs(motorSpeed); // maker sure value is positive
analogWrite(in1Pin, 0); // right motor
analogWrite(in2Pin, motorSpeed); // right motor
analogWrite(in3Pin, motorSpeed); // left motor
analogWrite(in4Pin, 0); // left motor
if (delayTime > 0) {
delay(delayTime);
stopMotor();
}
}
void goBackward(int delayTime = 0, int motorSpeed = 255) {
// change the 0 and motorSpeed for each pair so that the robot will go backward
motorSpeed = abs(motorSpeed); // maker sure value is positive
analogWrite(in1Pin, motorSpeed); // right motor
analogWrite(in2Pin, 0); // right motor
analogWrite(in3Pin, 0); // left motor
analogWrite(in4Pin, motorSpeed); // left motor
if (delayTime > 0) {
delay(delayTime);
stopMotor();
}
}
// you can use this to test your goForward() and goBackward() functions
void testMotors() {
Serial.println("Go forward");
goForward(1000);
Serial.println("Go backward");
goBackward(1000);
}
void setup() {
Serial.begin(115200);
digitalWrite(statusPin, HIGH);
initMotor();
// testMotors();
initMPU();
initPID();
// testGyro();
}
void loop() {
static float xyz[3];
flashActivityLed(500, 150, 1);
checkMPU(xyz);
// get X-axis value and adjust the upright position to be at 180
// choose the correct code from below - whether gyro is facing UP or DOWN
// facing UP
// this is for when the gyro is placed with the components side pointing UP
pidInput = xyz[0] + 180;
// facing DOWN
// this is for when the gyro is placed with the components side pointing DOWN
// pidInput = xyz[0];
// if (pidInput < 0) pidInput = (180 + pidInput) + 180;
pid.Compute(); // compute the new PID correction value
Serial.print(pidInput); Serial.print(" => "); Serial.print(pidOutput);
// control the motors depending on the robot tilt (X-axis)
// depending on how you mounted the motors, you might have to
// swap the forward and backward tests
if (pidOutput < 0) { // Falling towards the front
Serial.println(" forward");
goForward(0, -pidOutput);
} else if (pidOutput > 0) { // Falling towards the back
Serial.println(" backward");
goBackward(0, pidOutput);
} else {
Serial.println(" upright");
stopMotor();
}
}
