Self balancing robot PID loop issues - Page 1
Forum section: electronics / beginners

author: yashrk
author_rank: Frequent Contributor
timestamp: April 17, 2016, 03:39:56 am
content: 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, MPU6050I have found no other mpu6050 library/ code works with teensy due to its issues with the avr's wire libraryPhoto 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:7#include <Wire.h>8#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter910Kalman kalmanX; // Create the Kalman instance1112/* IMU Data */13int16_t accX, accY, accZ;14int16_t tempRaw;15int16_t gyroX, gyroY, gyroZ;1617float accXangle;//, accYangle; // Angle calculate using the accelerometer18float gyroXangle;//, gyroYangle; // Angle calculate using the gyro19float kalAngleX;//, kalAngleY; // Calculate the angle using a Kalman filter2021unsigned long timer;22uint8_t i2cData; // Buffer for I2C data23float CurrentAngle;2425// Motor controller pins26const int AIN1 = 3; // (pwm) pin 3 connected to pin AIN127const int AIN2 = 9; // (pwm) pin 9 connected to pin AIN228const int BIN1 = 10; // (pwm) pin 10 connected to pin BIN129const int BIN2 = 11; // (pwm) pin 11 connected to pin BIN23031int speed;3233// PID34const float Kp = 4;35const float Ki = 1;36const float Kd = 1;37float pTerm, iTerm, dTerm, integrated_error, last_error, error;38const float K = 1.9*1.12;39#define GUARD_GAIN 10.04041#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))424344void setup() {45pinMode(AIN1, OUTPUT); // set pins to output46pinMode(AIN2, OUTPUT);47pinMode(BIN1, OUTPUT);48pinMode(BIN2, OUTPUT);49Serial.begin(57600);50Wire.begin();51i2cData = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz52i2cData = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling53i2cData = 0x00; // Set Gyro Full Scale Range to ±250deg/s54i2cData = 0x00; // Set Accelerometer Full Scale Range to ±2g55while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once56while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode5758while(i2cRead(0x75,i2cData,1));59if(i2cData != 0x68) { // Read "WHO_AM_I" register60Serial.print(F("Error reading sensor"));61while(1);62}6364delay(100); // Wait for sensor to stabilize6566/* Set kalman and gyro starting angle */67while(i2cRead(0x3B,i2cData,6));68accX = ((i2cData << 8) | i2cData);69accY = ((i2cData << 8) | i2cData);70accZ = ((i2cData << 8) | i2cData);71accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;7273kalmanX.setAngle(accXangle); // Set starting angle74gyroXangle = accXangle;75timer = micros();76}7778void loop() {79runEvery(25) // run code @ 40 Hz80{81dof();82if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)83{84stop();85}86else{87if (CurrentAngle < 230 && CurrentAngle > 130)88{89Pid();90Motors();91}92else93{94stop();95}96}97}98}99100void Motors(){101if (speed > 0)102{103//forward104analogWrite(AIN1, speed);105analogWrite(AIN2, 0);106analogWrite(BIN1, speed);107analogWrite(BIN2, 0);108}109else110{111// backward112speed = map(speed,0,-255,0,255);113analogWrite(AIN1, 0);114analogWrite(AIN2, speed);115analogWrite(BIN1, 0);116analogWrite(BIN2, speed);117}118}119120void stop()121{122analogWrite(AIN1, 0);123analogWrite(AIN2, 0);124analogWrite(BIN1, 0);125analogWrite(BIN2, 0);126}127128void Pid(){129error = 180 - CurrentAngle; // 180 = level130pTerm = Kp * error;131integrated_error += error;132iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);133dTerm = Kd * (error - last_error);134last_error = error;135speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);136}137138void dof()139{140while(i2cRead(0x3B,i2cData,14));141accX = ((i2cData << 8) | i2cData);142accY = ((i2cData << 8) | i2cData);143accZ = ((i2cData << 8) | i2cData);144tempRaw = ((i2cData << 8) | i2cData);145gyroX = ((i2cData << 8) | i2cData);146gyroY = ((i2cData << 8) | i2cData);147gyroZ = ((i2cData << 8) | i2cData);148accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;149double gyroXrate = (double)gyroX/131.0;150CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);151timer = micros();152}

author: yashrk
author_rank: Frequent Contributor
timestamp: April 17, 2016, 03:44:11 am
content: 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

author: Rerouter
author_rank: Super Contributor
timestamp: April 17, 2016, 04:05:41 am
content: 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,

author: yashrk
author_rank: Frequent Contributor
timestamp: April 17, 2016, 12:59:53 pm
content: I can easily increase the code frequencyThe nudges you are talking about can you point me to a similar implementationOr can you explain more in terms of its implementation in the code.

author: Rerouter
author_rank: Super Contributor
timestamp: April 17, 2016, 08:37:36 pm
content: 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,

author: yashrk
author_rank: Frequent Contributor
timestamp: April 20, 2016, 07:52:59 pm
content: 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.

author: suicidaleggroll
author_rank: Super Contributor
timestamp: April 20, 2016, 09:10:30 pm
content: 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?

author: rstofer
author_rank: Super Contributor
timestamp: April 20, 2016, 09:35:04 pm
content: 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.