Responding to your private message .......
The original code will actually work (despite the flaws in it). However, instead of setting PID values, I think it is better (at first) to use the screw-driver and potentiometers to set them. A procedure is used to set those values. Best to start with kp, followed by kd, then ki, in that order.
The code I have included in the frame below is nearly the same as the original one - except I have removed the 'receive' (remote control) code....since we don't need that yet.
The 'lastTime' parameter is now initialised to zero (at least). Also note - the part that says " if(timeChange >= SampleTime) " will always be true, so that part of the code will always be executed (regardless), because the author incorrectly used different time units for "timeChange" and "SampleTime". But that's ok.... it'll still work even if you don't fix it. Otherwise, you can simply remove that 'if' statement (and its associated brackets), which will allow the code within it to execute every time. But, as mentioned, it'll execute every time already.
The original code comes from: SainSmart | Desktop CNC, 3D Printing & DIY Tools | Power to the Makers – SainSmart.com
I changed some things in the code - shown in the frame below. However, accelerometer calibrations should be done too. So, apart from needing a correction value for one of the gyroscope values (Gry_offset), the same kind of correction values need to be found for ay and az readings. These can only be obtained through actual measurements done on the MPU6050 accelerometers. So, before even doing anything with the bot, accelerometer measurements need to be made to ensure that this sensor device is working properly - giving sensible readings.
Getting the bot to balance well requires both the hardware and the software control algorithms to be working properly. After doing the sensor testing, it is usually best to remove all the Serial.print commands, which can probably slow the control system and impact the performance.
#include "Wire.h"
#include "SPI.h"
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.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 20
#define LMotor_offset 20
#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 = 0; //initialised
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
int TN1=3;
int TN2=4;
int ENA=9;
int TN3=5;
int TN4=6;
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);
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setRADDR((byte *)"serv1");
Mirf.payload = sizeof(long);
Mirf.config();
Serial.begin(115200);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega = Gyr_Gain * (gx + Gry_offset); Serial.print(" omega="); Serial.print(omega);
if (abs(r_angle)<45){
myPID();
PWMControl();
}
else{
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
}
void myPID(){
kp = analogRead(A0)*0.1; Serial.print(" kp=");Serial.print(kp);
kd = analogRead(A2)*1.0; Serial.print(" kd=");Serial.print(kd);
ki = analogRead(A1)*0.001; Serial.print(" ki=");Serial.print(ki);
//kp = 0; Serial.print(" kp=");Serial.print(kp);
//kd = 0; Serial.print(" kd=");Serial.print(kd);
//ki = 0; Serial.print(" ki=");Serial.print(ki);
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0; //changed preTime to lastTime
lastTime = now; //changed preTime to lastTime
float K = 0.98; //modified
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle; Serial.print(" f_angle=");Serial.print(f_angle);
timeChange = (now - lastTime);
if(timeChange >= SampleTime){
Input = f_angle;
error = Input;
errSum += error * timeChange;
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr;
LOutput = Output + Run_Speed + Turn_Speed; Serial.print(" LOutput=");Serial.print(LOutput);
ROutput = Output + Run_Speed - Turn_Speed; Serial.print(" ROutput=");Serial.println(ROutput);
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, HIGH);
digitalWrite(TN2, HIGH);
}
if(ROutput > 0){
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
else if(ROutput < 0){
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
else{
digitalWrite(TN3, HIGH);
digitalWrite(TN4, HIGH);
}
analogWrite(ENA, min(255, abs(LOutput) + LMotor_offset));
analogWrite(ENB, min(255, abs(ROutput) + RMotor_offset));
}