Hi guys I have been working some time on balance bot where I puted together Arduino and NXT. I wrote library for nxt motor which works quite well! It use PID control to reach the speed which we wants(that works).
Next task was to implement pid for balancing that is allready harder. I CAN NOT find proper values of PID control. I read and tryed i think everything what is possible! But the bot is like stupid it just tilts from one to other side. I am posting here my code with hope that somebody will be so nice and will help me thank you!!
I am using MPU6050 with DMP angle which work really good so there is not a problem.
MY PID CODE:
int sampleTime =9;
unsigned long lastTime = 0;
double output;
double input;
double setpoint = 0;
double error;
double iTerm, lastValue,pTerm,dTerm;
int updatePid(double targetValue, double currentValue) {
if (!inAuto)
return 0;
/*How long since we last calculated*/
unsigned long now = millis();
float pidTerm;
int timeChange = (now - lastTime);
if (timeChange >= sampleTime) {
/*Compute all the working error variables*/
error = targetValue - currentValue;
iTerm += Ki * error;
iTerm = constrain(iTerm, -50, 50);
pTerm = Kp * error;
dTerm = Kd * (currentValue - lastValue);
/*Compute PID Output*/
pidTerm = (pTerm - dTerm + iTerm) * K;
lastValue = currentValue;
lastTime = now;
}
return constrain(round(pidTerm), -255, 255);
}
loop(){
...........
input = (ypr[2] * 180 / M_PI);
output = updatePid(setpoint,input);
AmotorNXT.setMotor(output);
BmotorNXT.setMotor(output);
............
}
I also wrote GUI to read and set values of PID and make some graphs of it! I attache code also.
And photo of my bot
I will be happy for all kinds of help! Thank you guys
MotorNXT.cpp (3.25 KB)
MotorNXT.h (1.68 KB)
GUI_graph.rar (13.5 KB)
