Segway Clone - Problem with only positive angle

Hi everybody
My next project is a clone of the Segway, I’m using this hardware:

  • MPU6050
  • Arduino Uno
  • 25x2 Sabertooth

The problem is that the output of the engine that uses variable “Output” is never negative, this means that only the Segway was just sideways, it only goes to one direction ant it falls. I can’t find the error.
Can someone help me?

Here is the code:

#include "Wire.h"

#include "I2Cdev.h"
#include "MPU6050.h"
#include <math.h>

MPU6050 mpu;

//Accel, Gyro, Mag LSB/RAW values  16-bit
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;

//Accel, Gyro, Mag sensitivities , and other constants
const float accel_s = 16384; //LSB/g
const float gyro_s = 131;  //LSB/(degrees/s) //131
const float Pi = 3.141593;
const int outMax = 255;
const int outMin = 0;

//Complementary Filter variables
double accel_x, accel_y, accel_z; //acceleration in x,y,z direction
double accel;  //acceleration of gravity
double accelAngle;
double gyroAngleAccel;
unsigned long time; //to find dt
unsigned long old_time; //to find dt
double dt; //miliseconds
double angle;

//Motor Variables
int DIR_A = 4; //Determines direction of Motor A
int PWM_A = 5; //Determines power of Motor A
int DIR_B = 7; //Determines direction of Motor B
int PWM_B = 6; //Determines power of Motor B

//PID Control Variables
double uprightAngle = 0; //the empirically deduced upright position of the robot where pitch = 0
double tilt; //tilt = 0 when robot is at upright angle, therefore tilt = angle - uprightAngle
double oldTilt;
double sumTilt; // sum of all the tilts
int kp = 4.5; //Constant for proportional control
int ki = 0.3; //Constant for integrative control
int kd = 1; //Constant for derivative control
int Output; //Output for motor
int motorA = 0;
int motorB = 0;

int stat_mpu = 1; //Estat del sensor MPU6050
boolean MPU6050_prepatat = false;

void setup(){
  
    Wire.begin();
    Serial.begin(115200);

    Serial.println(F("..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Inician conexio MPU6050..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 ok") : F("MPU6050 failed.."));

    // wait for ready
    Serial.println(F("\nClica qualsevol tecla per iniciar el programa: "));
    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
    Serial.println(F("Initializing DMP..."));
    stat_mpu = 0;
    
    if (stat_mpu == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);


       

        Serial.println(F("MPU6050 prepared! Waiting for the connection..."));
        MPU6050_prepatat = true;

        // get expected DMP packet size for later comparison
       
    } else {
        Serial.print(F("MPU6050 no conection(code "));
        
        Serial.println(F(") --- STOP ---"));
        delay(10000);
    }
  
  delay(2000);
  //Setup Channel A
  pinMode(PWM_A, OUTPUT); //Initiates Motor Channel A pin
  pinMode(DIR_A, OUTPUT); //Initiates Direction Channel A pin

pinMode(A1, INPUT);
  //Setup Channel B
  pinMode(PWM_B, OUTPUT); //Initiates Motor Channel B pin
  pinMode(DIR_B, OUTPUT);  //Initiates Direction Channel B pin
  time = millis();
  angle = uprightAngle; //stable at 87.5
  tilt = 0;
  sumTilt = 0; //Integral of all calculated tilt,if this is perpetually high, change setpoint
}
void loop(){

    // calculating dt  
    old_time = time;
    time = millis();
    dt = (time - old_time)/1000;
    
    // getting the axes of motion (divide variable by accelerometer sensitivity
    mpu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    accel_x = ax/accel_s; 
    accel_y = ay/accel_s;
    accel_z = az/accel_s;
    
    //Processing Accelerometer and Gyroscope Data into pitch in degrees/ degrees per second
    accel = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2));
    accelAngle = (acos (accel_z/accel))*180/Pi; 
    gyroAngleAccel = gy/gyro_s; //gyroscope measurement of pitch is divided by sensitivity to yield degress rotation per second
    
    angle = (0.98)*(angle + gyroAngleAccel*(dt)) + (0.02)*(accelAngle); //Complementary Filter
    
    // All this is PID
    oldTilt = tilt;
    tilt = angle - uprightAngle; //how much robot is deviating from upright position
    sumTilt += (ki * tilt); // tilt can be positive or negative, hopefully stays near 0

    if(sumTilt > outMax) sumTilt = outMax; // to prevent over accumulation of integral value
    else if(sumTilt < outMin) sumTilt = outMin;
    
    Output = kp*tilt + sumTilt - kd*(tilt - oldTilt)*dt; //applying the PID Control algorithm
    
    if(Output > 0) Output += 80;
    else if(Output < 0) Output -= 80;
    if(Output > outMax) Output = outMax;
    else if(Output < outMin) Output = outMin;

     Serial.print(F(" TILT: ")); 
  Serial.print(tilt);
  Serial.print(F(" ANGLE: ")); 
  Serial.print(angle);
   Serial.print(F(" ACCEL: ")); 
  Serial.print(accel);
  Serial.print(F(" GYRO: ")); 
  Serial.println(gyroAngleAccel);

    if (Output >= 0){
      Serial.print("a");
      motorA = map(Output, 0, 255, 128, 255);
      PORTD &=~_BV(PD7);
      //digitalWrite(7, HIGH); //Establishes forward direction of Channel A
      analogWrite(6, motorA);   //Spins the motor on Channel A at full speed

       motorB = map(Output, 0, 255, 128, 255);
      PORTD |=_BV(PD4);
      //digitalWrite(4, LOW); //Establishes forward direction of Channel A
      analogWrite(5, motorB);   //Spins the motor on Channel A at full speed
    }
  
    else{
      Serial.print("b");
      motorA = map(-Output, 0, 255, 180, 0);
      PORTD |=_BV(PD7);
      //digitalWrite(7, LOW); //Establishes forward direction of Channel A
      analogWrite(6, motorA);   //Spins the motor on Channel A at full speed

      motorB = map(-Output, 0, 255, 128, 0);
      PORTD &=~_BV(PD4);
      //digitalWrite(4, HIGH); //Establishes forward direction of Channel A
      analogWrite(5, motorB);   //Spins the motor on Channel A at full speed
    }
Serial.print(Output);
    
}

And here is the output from de Serial Port:

 TILT: 4.64 ANGLE: 4.64 ACCEL: 1.00 GYRO: 5.27 OUTPUT MOTORS: a98
 TILT: 4.68 ANGLE: 4.68 ACCEL: 1.00 GYRO: 4.02 OUTPUT MOTORS: a98
 TILT: 4.72 ANGLE: 4.72 ACCEL: 1.01 GYRO: 3.03 OUTPUT MOTORS: a98
 TILT: 4.77 ANGLE: 4.77 ACCEL: 1.01 GYRO: 1.85 OUTPUT MOTORS: a99
 TILT: 4.80 ANGLE: 4.80 ACCEL: 1.00 GYRO: 0.72 OUTPUT MOTORS: a99
 TILT: 4.84 ANGLE: 4.84 ACCEL: 1.00 GYRO: -0.05 OUTPUT MOTORS: a99
 TILT: 4.88 ANGLE: 4.88 ACCEL: 1.00 GYRO: -0.88 OUTPUT MOTORS: a99
 TILT: 4.91 ANGLE: 4.91 ACCEL: 1.00 GYRO: -1.36 OUTPUT MOTORS: a99
 TILT: 4.94 ANGLE: 4.94 ACCEL: 1.01 GYRO: -2.41 OUTPUT MOTORS: a99
 TILT: 4.98 ANGLE: 4.98 ACCEL: 1.00 GYRO: -4.15 OUTPUT MOTORS: a99
 TILT: 5.00 ANGLE: 5.00 ACCEL: 1.00 GYRO: -5.05 OUTPUT MOTORS: a99
 TILT: 5.01 ANGLE: 5.01 ACCEL: 1.02 GYRO: -7.00 OUTPUT MOTORS: a100
 TILT: 5.00 ANGLE: 5.00 ACCEL: 1.01 GYRO: -11.13 OUTPUT MOTORS: a99
 TILT: 5.00 ANGLE: 5.00 ACCEL: 1.02 GYRO: -15.60 OUTPUT MOTORS: a99
 TILT: 5.03 ANGLE: 5.03 ACCEL: 1.01 GYRO: -17.11 OUTPUT MOTORS: a100
 TILT: 5.09 ANGLE: 5.09 ACCEL: 0.98 GYRO: -15.53 OUTPUT MOTORS: a100
 TILT: 5.14 ANGLE: 5.14 ACCEL: 0.98 GYRO: -13.10 OUTPUT MOTORS: a100
 TILT: 5.17 ANGLE: 5.17 ACCEL: 0.97 GYRO: -12.44 OUTPUT MOTORS: a100
 TILT: 5.18 ANGLE: 5.18 ACCEL: 1.00 GYRO: -11.98 OUTPUT MOTORS: a100
 TILT: 4.58 ANGLE: 4.58 ACCEL: 1.01 GYRO: 12.77 OUTPUT MOTORS: a98
 TILT: 4.54 ANGLE: 4.54 ACCEL: 1.01 GYRO: 9.03 OUTPUT MOTORS: a98
 TILT: 4.53 ANGLE: 4.53 ACCEL: 0.98 GYRO: 7.39 OUTPUT MOTORS: a98
 TILT: 4.52 ANGLE: 4.52 ACCEL: 1.00 GYRO: 6.93 OUTPUT MOTORS: a98
 TILT: 4.51 ANGLE: 4.51 ACCEL: 1.00 GYRO: 6.34 OUTPUT MOTORS: a98
 TILT: 4.49 ANGLE: 4.49 ACCEL: 1.01 GYRO: 4.56 OUTPUT MOTORS: a97
 TILT: 4.47 ANGLE: 4.47 ACCEL: 1.01 GYRO: 2.50 OUTPUT MOTORS: a97
 TILT: 4.48 ANGLE: 4.48 ACCEL: 1.00 GYRO: 3.63 OUTPUT MOTORS: a97
 TILT: 4.51 ANGLE: 4.51 ACCEL: 0.96 GYRO: 4.79 OUTPUT MOTORS: a98
 TILT: 4.51 ANGLE: 4.51 ACCEL: 1.01 GYRO: 8.44 OUTPUT MOTORS: a98
 TILT: 4.52 ANGLE: 4.52 ACCEL: 1.02 GYRO: 7.85 OUTPUT MOTORS: a98
 TILT: 4.48 ANGLE: 4.48 ACCEL: 1.04 GYRO: 5.43 OUTPUT MOTORS: a97
 TILT: 4.44 ANGLE: 4.44 ACCEL: 0.99 GYRO: 2.33 OUTPUT MOTORS: a97
    else if(Output < outMin) Output = outMin;

If Output is negative, make it 0. Then, wonder why Output isn’t negative. Hmmm…