Self balancing robot PID loop issues

[yashrk]

Hey guys, 
        I been working on self balancing robot for my college project.  It's almost done, hardware is working and software is working but I am getting issues with the PID part it balances with bit oscillations which is ohk for such a system but after a certain time this oscillations starts to increase slowly at the certain point it is travelling meters in its oscillations and then ultimately it falls.
       I have tried different values last night but still it wasn't stable, in some cases it will balance the bot perfectly but start to move forward initially with low but increasing  acceleration and ultimately falls down.

I am using teensy 3.2,  MPU6050

I have found no other mpu6050 library/ code works with teensy due to its issues with the avr's wire library

Photo of the hardware is added on this link- https://yashrk.wordpress.com/2016/04/13/2-update-mini-project/

Code base on - http://www.bajdi.com/building-a-self-balancing-bot/

Code: [Select]

 
7
#include <Wire.h>
8
#include "Kalman.h" // Source: [url]https://github.com/TKJElectronics/KalmanFilter[/url]
9
 
10
Kalman kalmanX; // Create the Kalman instance
11
 
12
/* IMU Data */
13
int16_t accX, accY, accZ;
14
int16_t tempRaw;
15
int16_t gyroX, gyroY, gyroZ;
16
 
17
float accXangle;//, accYangle; // Angle calculate using the accelerometer
18
float gyroXangle;//, gyroYangle; // Angle calculate using the gyro
19
float kalAngleX;//, kalAngleY; // Calculate the angle using a Kalman filter
20
 
21
unsigned long timer;
22
uint8_t i2cData[14]; // Buffer for I2C data
23
float CurrentAngle;
24
 
25
// Motor controller pins
26
const int AIN1 = 3;  // (pwm) pin 3 connected to pin AIN1
27
const int AIN2 = 9;  // (pwm) pin 9 connected to pin AIN2
28
const int BIN1 = 10; // (pwm) pin 10 connected to pin BIN1 
29
const int BIN2 = 11;  // (pwm) pin 11 connected to pin BIN2
30
 
31
int speed;
32
 
33
// PID
34
const float Kp = 4;
35
const float Ki = 1;
36
const float Kd = 1;
37
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
38
const float K = 1.9*1.12;
39
#define   GUARD_GAIN   10.0
40
 
41
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
42
 
43
 
44
void setup() { 
45
  pinMode(AIN1, OUTPUT); // set pins to output
46
  pinMode(AIN2, OUTPUT);
47
  pinMode(BIN1, OUTPUT);
48
  pinMode(BIN2, OUTPUT);
49
  Serial.begin(57600);
50
  Wire.begin();
51
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
52
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
53
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
54
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
55
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
56
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
57
 
58
    while(i2cRead(0x75,i2cData,1));
59
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
60
    Serial.print(F("Error reading sensor"));
61
    while(1);
62
  }
63
 
64
  delay(100); // Wait for sensor to stabilize
65
 
66
  /* Set kalman and gyro starting angle */
67
  while(i2cRead(0x3B,i2cData,6));
68
  accX = ((i2cData[0] << 8) | i2cData[1]);
69
  accY = ((i2cData[2] << 8) | i2cData[3]);
70
  accZ = ((i2cData[4] << 8) | i2cData[5]);
71
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
72
 
73
  kalmanX.setAngle(accXangle); // Set starting angle
74
  gyroXangle = accXangle;
75
  timer = micros();
76
}
77
 
78
void loop() {
79
  runEvery(25)  // run code @ 40 Hz
80
  {
81
    dof();
82
    if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)
83
    {
84
      stop();
85
    }
86
    else{
87
    if (CurrentAngle < 230 && CurrentAngle > 130)
88
    {
89
    Pid();
90
    Motors();
91
    }
92
    else
93
    {
94
      stop();
95
    }
96
  }
97
  }
98
}
99
 
100
void Motors(){
101
  if (speed > 0)
102
  {
103
    //forward
104
    analogWrite(AIN1, speed);
105
    analogWrite(AIN2, 0);
106
    analogWrite(BIN1, speed);
107
    analogWrite(BIN2, 0);
108
  }
109
  else
110
  {
111
    // backward
112
    speed = map(speed,0,-255,0,255);
113
    analogWrite(AIN1, 0);
114
    analogWrite(AIN2, speed);
115
    analogWrite(BIN1, 0);
116
    analogWrite(BIN2, speed);
117
  }
118
}
119
 
120
void stop()
121
{
122
  analogWrite(AIN1, 0);
123
  analogWrite(AIN2, 0);
124
  analogWrite(BIN1, 0);
125
  analogWrite(BIN2, 0);
126
}
127
 
128
void Pid(){
129
  error = 180 - CurrentAngle;  // 180 = level
130
  pTerm = Kp * error;
131
  integrated_error += error;
132
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
133
  dTerm = Kd * (error - last_error);
134
  last_error = error;
135
  speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
136
}
137
 
138
void dof()
139
{
140
  while(i2cRead(0x3B,i2cData,14));
141
  accX = ((i2cData[0] << 8) | i2cData[1]);
142
  accY = ((i2cData[2] << 8) | i2cData[3]);
143
  accZ = ((i2cData[4] << 8) | i2cData[5]);
144
  tempRaw = ((i2cData[6] << 8) | i2cData[7]); 
145
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
146
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
147
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
148
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
149
  double gyroXrate = (double)gyroX/131.0;
150
  CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
151
  timer = micros();
152
}

[yashrk]

The code is not the one which is working on my bot but it's almost same with difference in pinouts and motor driving part

[Rerouter]

To really servo it, you cannot just have it stop at 180 degrees, as that doesnt account for when its traversing that deadzone, you still want very tiny nudges to hold it at that angle, as otherwise it stops dead in its tracks for 50ms and cruises on past its target and has to fight to correct, (25ms from last correction nudging it through the deadzone to when its in there, and another 25ms before it can respond to exiting it,)

equally if your hardware allows it i would increase the correction rate greatly with lower gains and a faster I, and a tiny D, this should keep it more stable,

To my knowledge most of these approaches use a variable timed pulse approach on the output h bridge, so its never varying the voltage or current available to the motor, but purely adjusting the pulse width, so the faster your loop is, the more efficient it gets as really tiny pulses into motors dont give the inductor time to allow significant current to flow, while still giving a very tiny nudge,

[yashrk]

I can easily increase the code frequency

The nudges you are talking about can you point me to a similar implementation
Or can you explain more in terms of its implementation in the code.

[Rerouter]

Line 81 onward of your code, you literally stop the control loop if your inside a certain angle range, where in reality this is exactly the time you want your derivative term to stabilize the thing at that angle,

[yashrk]

 
I increased code frequency and also removed the stop sequence from the code and tried stabilizing it by trying different PID values but still it is not stable.

[suicidaleggroll]

Do you have any method to deriving these new PID values, or are you just throwing random numbers in to see if anything fixes it?  How familiar are you with the intricacies of PID controllers, what each term controls, how it affects the q-factor of the final result, etc?  Are you recording the bot's tilt angle as well as the P, I, and D contributions in the control loop as a function of time so you can analyze the data mathematically?

[rstofer]

Not directly on point but this fellow has gotten self-balancing down to a fine art.  Look at how the robot responds to large disturbances and then settles down. His earlier attempts used 3 uCs - one for measuring wheel motion, one for handling the IMU and one for combining the data and controlling the servos.  For this incantation, he has moved to an ARM processor(Arduino Due):

http://hackaday.com/2012/07/20/self-balancing-robot-uses-cascading-pid-algorithms/

http://sebastiannilsson.com/en/k/projekt/selfbalancing-robot/

One thing he does is change the gain of the balancing PID when the bot is near equilibrium.  This gives him a lot of gain when the IMU says he is in trouble and not so much when the robot settles down.