I use an mpu6050 to get the pitch and a pid library for pid calculation. I have seen the motors that I use in other selfbalancing robots so they are powerful enough.
I think there is a problem with the code because it looks like it doesn't calculate the pwm value fast enough and falls and then spins the motors like crazy.
What is wrong with my code?
#define _USE_MATH_DEFINES
#include <cmath>
#include <math.h>
#include "Arduino.h"
#include "I2Cdev.h"
#include <AutoPID.h>
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>
#include "MPU6050_6Axis_MotionApps20.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
//#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
//#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
#define INTERRUPT_PIN 4 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 2
#define SCL 22
#define SDA 21
#define OUTPUT_MIN -255
#define OUTPUT_MAX 255
double KP=5;
double KI=0;
double KD=0;
#define enA 19 //PWM output
#define enB 18 //PWM output
#define in1 5
#define in2 17
#define in3 16
#define in4 12
#define freq 5000
#define channel 0
#define resolution 8
double pwm, input;
double setPoint=0.00;
bool blinkState = false;
// 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
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
AutoPID balance(&input, &setPoint, &pwm, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);
void setup() {
Dabble.begin("MyEsp32"); //set bluetooth name of your device
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin(SDA,SCL);
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(SDA,SCL,400, true);
#endif
Serial.begin(115200);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT_PULLUP);
while(GamePad.isTrianglePressed()==0){
Dabble.processInput();
}
//while (Serial.available() && Serial.read()); // empty buffer
//while (!Serial.available()); // wait for data
//while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
devStatus = mpu.dmpInitialize();
// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro
//OFFSETS -1650, -1233, 2850, -1996, -74, -43
// supply your own gyro offsets here, scaled for min sensitivity
// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro
//OFFSETS -1650, -1331, 2860, -2041, -72, -38
mpu.setXAccelOffset(-1650);
mpu.setYAccelOffset(-1331);
mpu.setZAccelOffset(2860);
mpu.setXGyroOffset(-2041);
mpu.setYGyroOffset(-72);
mpu.setZGyroOffset(-38);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
ledcSetup(channel, freq, resolution);
ledcAttachPin(enA, channel);
ledcAttachPin(enB, channel);
//balance.setBangBang(0.40);
//balance.setTimeStep(4000);
}
void loop() {
// 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) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
Serial.print("p\t");
Serial.print(input);
balance.run();
Serial.print("pwm\t");
Serial.println(pwm);
if(pwm>0){
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
ledcWrite(channel, abs(pwm));
}else if(pwm<0){
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
ledcWrite(channel, abs(pwm));
}
}
// 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("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);
// 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);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input=ypr[2] * 180/M_PI;
}
}
