Self balancing robot

Community Jobs and Paid Consultancy
1

Hey! I just started working on a self balancing robot. The robot is ready, but for the code i am having some issues.

I wrote a code where just by measuring the tilt it controlls the speed of the motor. However it is able to balance just for a second or two.

#include <Wire.h>
const int MPU_addr=0x68; 
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int minVal=265; int maxVal=402;
double x;         //A4 (SDA), A5 (SCL)
double y; 
double z;
int lmt1=6;
int lmt2=5;
int rmt1=10;
int rmt2=9;

int value;

void setup(){
  pinMode(lmt1,OUTPUT);
  pinMode(lmt2,OUTPUT);
  pinMode(rmt1,OUTPUT);
  pinMode(rmt2,OUTPUT);
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B); Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600); } 

void loop(){ 
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
Wire.endTransmission(false); 
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read(); 
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read(); 
int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
 
//-------------form here the changing of the x value is because the gyroscope is mounted vertically
//                 so reucing value to 86(which is exact.). and other lines to modify the angle as the gyro as 
//                 it shows value from 0 to 360 but modified it to -180 to 180.
x=x-86;
if(x<0){
  x=x+360;
}
if(x>1){
if(x<180){
  x=round(x);
  value=map(x,0,50,0,150);
  if(value>255){
    value=255;
  }
  Serial.println(value);
  analogWrite(lmt1,value);
  analogWrite(lmt2,0);
  analogWrite(rmt1,value);
  analogWrite(rmt2,0);
}
else if(x>180){
  value=x-360;
  value=value*(-1);
  value=map(value,0,50,0,145);
  if(value>255){
    value=255;
  }
  Serial.println(value);
  analogWrite(lmt1,0);
  analogWrite(lmt2,value);
  analogWrite(rmt1,0);
  analogWrite(rmt2,value);
}
}
delay(20);
 }

if you don't understand the code its just the fact that if the robot tilts froward the robot goes forward. It works perfectly do no problem in the code. The problem it that it like continuously adjust making it super unstable. And i am using only forward and back motion but in the internet i say usage of left and right.

I don't think I am using the right technique to balance the robot.
Other robot tutorial use stepper or other motors. I am using a simple DC motor.

Could anyone help me with the program.

Thanks

2 
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();

If you ask for 14 bytes, you are expected to read all 14 of them. Reading only 6 of them will result in 8 being left for the next time you read.

Of course, having asked for 14, and expecting there to be 14 to read, immediately, may not be reasonable.

Nor is reading 6 bytes without knowing that there ARE 6 bytes to read.

if(x>1){
if(x<180){
  x=round(x);
  value=map(x,0,50,0,150);
  if(value>255){
    value=255;
  }

If the value is between 1 and 180, map it as if it was on the range 0 to 50, to a range of 0 to 150 (i.e. multiply the value by 3), then constrain it to be less than or equal to 255. Just WHY you are doing that should be documented.

We can't help you make the robot balance without seeing just what your robot looks like.