Hi guys PLEASE I NEED HELP!!!
I am wondering how can I control for exampel balancing robot which is controlled by PID algorythm with joystick?
The PID controll tryes to balance the robot and how can I than controll it with joystick? Like when I tilt the button on joystick that it should go streight , than the PID controll will not try to defuse the tild?
PLEASE HELP THANK YOU SO MUCH!!
Have you got the balancing part working ? Code ? Hardware ?
Not yet.I just made mpu6050 work. But i am asking how does it work generally. How can i make move robot which is controlled by PID which i think will work against my order move. I would like to understand it if you know some tutorial for this or when i can learn it?
Thank you so much for response!!
How can i make move robot which is controlled by PID which i think will work against my order move.
Your joystick would be used to adjust the Setpoint value that the PID controller is aiming to achieve.
Thank you this is the answere which I was searching for.
But i found out one more problem which I have with PID control.
Is it correct that PID value is increasing all the time? For exampel when I have seted in my MPU6050 as Setpoint 180degrees and when I tilt it on one side the PID value increase to infinity and when I tilt it sudenlly to another direction the PID value is just from last value decreasing.
For examepl,when I tilt it to right PID value is increasing to 1000 when I tilt it to left it is decreasing.
Is it correct or how can I from this value calculate how much should servo turne ?
Thank you for your help!!!!!
Is it correct that PID value is increasing all the time?
No. If you are measuring the right things, and using the data correctly, the PID process is generating an Output value that should serve to minimize the error with respect to the SetPoint.
Is it correct or how can I from this value calculate how much should servo turne ?
No, it is not correct. What it means is that the output of the PID process is NOT being used correctly to control the attitude of the robot.
Without seeing your robot, or your code, you're on your own.
Thank you for your response!! I little bit worked more on the code, and now is behaving little bit strange. I am using MPU6050 with library Kalman.h and PID control also with library
PID: Arduino Playground - PIDLibrary
Kalman: KalmanFilter/Kalman.h at master · TKJElectronics/KalmanFilter · GitHub
The MPU on flat surface has still values around 0,7. I dont know why if you could help me i would really appraise it!
The PID control is the completelly right value for the Angle value on the left column. Setpoint = 0. I would understand if the PID value would not be all the time 0 becasue of the inaccuracy.
The PID value starts at 0, but everytime when I tilt MPU in X direction(the first column) and back the PID value increases to one number and when i have it on table it allready is not 0 but another number,now 13!
code:
#include <PID_v1.h>
#include <Servo.h>
#include <Wire.h>
#include "Kalman.h"
#define COM_CONST1 0.99
#define COM_CONST2 0.1
#define LSBsensitivity 131
Kalman kalmanX;
Kalman kalmanY;
Servo xservo;
uint8_t IMUAddress = 0x68; // MPU6050 Address
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
int mapX;
double pitch; //accXangle
double roll; //accYangle
double gyroXangle = 9;
double gyroYangle = 180;
double kalAngleX;
double kalAngleY;
uint32_t timer;
//Define Variables we'll be connecting to
double Setpoint, Output,Output2;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
PID myPID(&kalAngleX, &Output2, &Setpoint, consKp, consKi, consKd, REVERSE);
PID myPID2(&kalAngleX, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
// ---------- VOID SETUP START -------------- /
void setup() {
Serial.begin(115200);
// xservo.attach(9);
// xservo.write(90);
Wire.begin();
i2cWrite(0x6B,0x00); // Disable sleep mode
if(i2cRead(0x75,1)[0] != IMUAddress) { // Read "WHO_AM_I" register
Serial.print(F("MPU-6050 with address 0x"));
Serial.print(IMUAddress,HEX);
Serial.println(F(" is not connected"));
while(1);
}
readValues();
rollpitch();
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
timer = micros();
Setpoint = 0;
myPID.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);
}
void loop() {
/* Update all the values */
readValues();
KalmanValue();
double gap = abs(Setpoint-kalAngleX); //distance away from setpoint
if(gap<10)
{ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKp, consKi, consKd);
myPID2.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
myPID2.SetTunings(aggKp, aggKi, aggKd);
}
myPID.Compute();
myPID2.Compute();
Serial.print(kalAngleX);
Serial.print("\t");
Serial.print(kalAngleY);
Serial.print("\t");
Serial.print(Output+Output2);
Serial.print("\t");
Serial.print("\r\n");
delay(20); // delay to allow servos to move (ms)
}
void moveServo()
{
mapX = map(kalAngleX, 80, 280, 0, 179); //calculate limitation of servo mechanical
Serial.print("\t");
xservo.write(mapX); // Send signal to servo
Serial.print(mapX);
Serial.print("\n");
}
void KalmanValue()
{
rollpitch();
double dt = (double)(micros()-timer)/1000000;
timer = micros();
double gyroXrate = ( (double) gyroX / LSBsensitivity ); // LSB Sensitivity 131 LSB/s Full scale range = +- 250 degree/s (datasheet)
double gyroYrate =( (double) gyroY / LSBsensitivity );
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt) +0.7; // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
}
void rollpitch()
{
roll = atan(accY / sqrt(pow(accX,2) + pow(accZ,2))) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(pow(accY,2) + pow(accZ,2))) * RAD_TO_DEG;
}
// I2C
void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data[i] = Wire.read();
return data;
}
void readValues()
{
uint8_t* data = i2cRead(0x3B,14);
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);
return ;
}
THANK YOU SO MUCH FOR HELP