Hi having problem with this a while now how can I solve , btw my code is about doing a self balance robot suing LQR
#include <MPU6050.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <MPU6050_9Axis_MotionApps41.h>
#include <helper_3dmath.h>
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
#define Pi 3.14159
#define EN34 6 //m2 enable
#define EN12 5 //m1 enable
#define M2neg 8
#define M2pos 9
#define M1neg 10
#define M1pos 11
float K1 = 1, K2 = 0.2394, K3 = -34.1812, K4 = -3.2186;
float x1, x2, x3, x4;
float input;
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroXCalli = 0, gyroYCalli = 0, gyroZCalli = 0;
long gyroXPresent = 0, gyroYPresent = 0, gyroZPresent = 0;
long gyroXPast = 0, gyroYPast = 0, gyroZPast = 0;
float rotX, rotY, rotZ;
float angelX = 0, angelY = 0, angelZ = 0;
long timePast = 0;
long timePresent = 0;
void setPwmFrequency(int pin, int divisor) {
byte mode;
if (pin == 5 || pin == 7 || pin == 9 || pin == 10) {
switch (divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 64: mode = 0x03; break;
case 256: mode = 0x04; break;
case 1024: mode = 0x05; break;
default: return;
}
if (pin == 5 || pin == 6) {
TCCR0B = TCCR0B & 0b11111000 | mode;
} else {
TCCR1B = TCCR1B & 0b11111000 | mode;
}
} else if (pin == 3 || pin == 11) {
switch (divisor) {
case 1: mode = 0x01; break;
case 8: mode = 0x02; break;
case 32: mode = 0x03; break;
case 64: mode = 0x04; break;
case 128: mode = 0x05; break;
case 256: mode = 0x06; break;
case 1024: mode = 0x07; break;
default: return;
}
TCCR2B = TCCR2B & 0b11111000 | mode;
}
}
void setUpMPU() {
// power management
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x6B); // Access the power management register
Wire.write(0b00000000); // Set sleep = 0
Wire.endTransmission(); // End the communication
// configure gyro
Wire.beginTransmission(0b1101000);
Wire.write(0x1B); // Access the gyro configuration register
Wire.write(0b00000000);
Wire.endTransmission();
// configure accelerometer
Wire.beginTransmission(0b1101000);
Wire.write(0x1C); // Access the accelerometer configuration register
Wire.write(0b00000000);
Wire.endTransmission();
}
void getGyroValues() {
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x43); // Access the starting register of gyro readings
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6); // Request for 6 bytes from gyro registers (43 - 48)
while (Wire.available() < 6); // Wait untill all 6 bytes are available
gyroXPresent = Wire.read() << 8 | Wire.read(); // Store first two bytes into gyroXPresent
gyroYPresent = Wire.read() << 8 | Wire.read(); // Store next two bytes into gyroYPresent
gyroZPresent = Wire.read() << 8 | Wire.read(); //Store last two bytes into gyroZPresent
}
void callibrateGyroValues() {
for (int i = 0; i < 5000; i++) {
getGyroValues();
gyroXCalli = gyroXCalli + gyroXPresent;
gyroYCalli = gyroYCalli + gyroYPresent;
gyroZCalli = gyroZCalli + gyroZPresent;
}
gyroXCalli = gyroXCalli / 5000;
gyroYCalli = gyroYCalli / 5000;
gyroZCalli = gyroZCalli / 5000;
}
void readAndProcessAccelData() {
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6);
while (Wire.available() < 6)
;
accelX = Wire.read() << 8 | Wire.read();
accelY = Wire.read() << 8 | Wire.read();
accelZ = Wire.read() << 8 | Wire.read();
processAccelData();
}
void processAccelData() {
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
}
void readAndProcessGyroData() {
gyroXPast = gyroXPresent; // Assign Present gyro reaging to past gyro reading
gyroYPast = gyroYPresent; // Assign Present gyro reaging to past gyro reading
gyroZPast = gyroZPresent; // Assign Present gyro reaging to past gyro reading
timePast = timePresent; // Assign Present time to past time
timePresent = millis(); // get the current time in milli seconds, it is the present time
void getAngularVelocity() {
rotX = gyroXPresent / 131.0;
rotY = gyroYPresent / 131.0;
rotZ = gyroZPresent / 131.0;
}
void calculateAngle() {
// same equation can be written as
// angelZ = angelZ + ((timePresentZ - timePastZ)(gyroZPresent + gyroZPast - 2gyroZCalli)) / (21000131);
// 1/(10002131) = 0.00000382
// 1000 --> convert milli seconds into seconds
// 2 --> comes when calculation area of trapezium
// substacted the callibated result two times because there are two gyro readings
angelX = angelX + ((timePresent - timePast) * (gyroXPresent + gyroXPast - 2 * gyroXCalli)) * 0.00000382;
angelY = angelY + ((timePresent - timePast) * (gyroYPresent + gyroYPast - 2 * gyroYCalli)) * 0.00000382;
angelZ = angelZ + ((timePresent - timePast) * (gyroZPresent + gyroZPast - 2 * gyroZCalli)) * 0.00000382;
}
void setup() {
pinMode(EN34, OUTPUT);
pinMode(EN12, OUTPUT);
pinMode(M2neg, OUTPUT);
pinMode(M2pos, OUTPUT);
pinMode(M1neg, OUTPUT);
pinMode(M1pos, OUTPUT);
//Serial.begin(115200);
Serial.println("Initialize MPU6050");
//checkSettings();
Serial.begin(9600);
Wire.begin();
setUpMPU();
callibrateGyroValues();
timePresent = millis();
}
void loop() {
//float Vector rawAccel = mpu.readRawAccel();
//float Vector normAccel = mpu.readNormalizeAccel();
//float input=(normAccel.YAxis-0.3)*10;
//Serial.println(input);
//x3=input/57.3;
// === Read acceleromter data === //
Serial.print(" Accel (g)");
float readAndProcessAccelData();
float readAndProcessGyroData();
getGyroValues(); // get gyro readings
getAngularVelocity(); // get angular velocity
calculateAngle(); // calculate the angle
input = rotY;
Serial.println(input);
x3 = input;
int pwm = abs(constrain(-K3 * x3 * 6.5, -254, 254));
analogWrite(EN34, pwm - 4);
analogWrite(EN12, pwm);
Serial.println(pwm);
if (input < -7) {
digitalWrite(M1pos, HIGH);
digitalWrite(M1neg, LOW);
digitalWrite(M2pos, HIGH);
digitalWrite(M2neg, LOW);
} else if (input > 7) {
digitalWrite(M1pos, LOW);
digitalWrite(M1neg, HIGH);
digitalWrite(M2pos, LOW);
digitalWrite(M2neg, HIGH);
} else {
digitalWrite(M1pos, LOW);
digitalWrite(M1neg, LOW);
digitalWrite(M2pos, LOW);
digitalWrite(M2neg, LOW);
}
}