Hi Friends,
I m focused on making a self Balancing Robot and eventually i made one.
Here is a short video of the robot.
Here is my code. I find the codes from web and modified for myself.
#include "Wire.h"
#include "SPI.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
//#define Gry_offset 0
//#define Gyr_Gain 0.00763358
#define Angle_offset 0
//#define RMotor_offset 30
//#define LMotor_offset 30
#define pi 3.14159
//long data;
//int x, y;
float kp, ki, kd;
float r_angle, f_angle, omega;
//float Turn_Speed = 0, Turn_Speed_K = 0;
//float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;
unsigned long preTime = 0;
//float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr, pTerm, iTerm, dTerm;
int timeChange;
int TN1=4;
int TN2=3;
int ENA=9;
int TN3=6;
int TN4=5;
int ENB=10;
void setup() {
Wire.begin();
accelgyro.initialize();
pinMode(TN1,OUTPUT);
pinMode(TN2,OUTPUT);
pinMode(TN3,OUTPUT);
pinMode(TN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
Serial.begin(115200);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ax, az) * 180 / pi + Angle_offset);
omega = (double) gy/131;
myPID();
PWMControl();
}
void myPID(){
kp = map(analogRead(A2),0,1023,0,5000)*0.1;
ki = map(analogRead(A0),0,1023,0,5000)*0.0001;
kd = map(analogRead(A1),0,1023,0,10000);
// kp=30.50 kd=532.00 ki=0.31
//kp=67.40;
//kd=488.00 ;
//ki=2.40;
Serial.print(" kp=");Serial.print(kp);
Serial.print(" kd=");Serial.print(kd);
Serial.print(" ki=");Serial.print(ki);
unsigned long now = millis();
float dt = (now - preTime) / 1000.0;
preTime = now;
f_angle = (1-dt) * (f_angle + omega * dt) + dt * r_angle;
Serial.print(" angle=");Serial.println(f_angle);
timeChange = (now - lastTime);
error = 2.60-f_angle;
//errSum += error * timeChange;
//if (abs(errSum)>500){errSum=500;}
dErr = (error - lastErr);
pTerm=kp*error;
//iTerm=ki*errSum;
iTerm=constrain(iTerm,-200,200);
iTerm+=error;
//if (f_angle==0){iTerm=0;}
//iTerm=constrain(iTerm,-255,255);
dTerm=kd*dErr;
Output = pTerm+ki*iTerm+dTerm;
if (Output<=-255)
{
Output=-255;
}
else if(Output>=255)
{
Output=255;
}
/*if (Output<0)
{
LOutput = Output + (-10);
ROutput = Output + 10;
}
else if (Output>0)
{
LOutput = Output + 10;
ROutput = Output + (-10);
}*/
LOutput = Output;
ROutput = Output;
lastErr = error;
lastTime = now;
}
void PWMControl(){
if(LOutput < 0){
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
}
else if(LOutput > 0){
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
}
else{
digitalWrite(TN1, LOW);
digitalWrite(TN2, LOW);
}
if(ROutput < 0){
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
else if(ROutput > 0){
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
else{
digitalWrite(TN3, LOW);
digitalWrite(TN4, LOW);
}
analogWrite(ENA, min(255, abs(LOutput)));
analogWrite(ENB, min(255, abs(ROutput)));
}
I used 500 rpm 25mm motors without encoders. MPU6050, 3 potentiometer, Arduino UNO and motor driver.
I spent hours to find the best pid values but unfortunately i couldnt. The best result of my robot is shown on the video. Normally these robots oscilates a few times and and the oscilation periods get smaller each period. But in my project it is getting higher by the time.
As a result, I couldnt find what's wrong with my robot.
Help me please if you have an dea about this problem. Is this problem comes from chasis?, or from motor torque or rpm?, or codes? or PID?, or complementary filter? ... etc.
Thanks for helps ![]()