Need some HELP with my pid

Projects Programming
1

Hi everyone, I'm currently building a little robot for some national competitions.....I wanted to equip it with pid since it would be very useful for making 90 degree turns without going over them....But I'm having quite a few problems making it work and I would have I need someone who knows about PID which has a bno055 as input......I attach here the code that I tried to make and which works but very badly......Thank you very much.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);

float target = 0;
double error = 0;
double proportional = 0;
double integral = 0;
double derivative = 0;
double last_error = 0;

double dt,last_time=0;

double Kp = 5;
double Ki = 0;
double Kd = 0;

int mtrSpd = 60;

float angle = 0;

void setup() {
  Serial.begin(9600);
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);
  pinMode(24, OUTPUT);
  pinMode(25, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(28, OUTPUT);
  pinMode(29, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);

  Wire.begin();
  bno.begin();

  digitalWrite(22, LOW); //motore RL
  digitalWrite(23, HIGH);
  digitalWrite(24, HIGH); //motore RR
  digitalWrite(25, LOW);
  digitalWrite(26, HIGH); //motore RL
  digitalWrite(27, LOW);
  digitalWrite(28, HIGH); //motore RR
  digitalWrite(29, LOW);

  TCCR1A = 0b10100001;
  TCCR1B = 0b00001101;
  OCR1A = mtrSpd;
  OCR1B = mtrSpd;

  TCCR2A = 0b10100011;
  TCCR2B = 0b00000111;
  OCR2A = mtrSpd;
  OCR2B = mtrSpd;

  bno.setExtCrystalUse(true);
}

void loop() {
  double now = millis();
  dt = (now - last_time)/1000.00;
  last_time=now;

  sensors_event_t event;
  bno.getEvent(&event);
  float angle_input = event.orientation.x;
  
  
  error = target - angle_input;
  proportional = error;
  integral = integral + (error*dt);
  derivative = (error - last_error)/dt;
  
  angle = (proportional * Kp) + (integral * Ki) + (derivative * Kd);
  
  Serial.print(target);
  Serial.print(",");
  Serial.print(angle_input);
  Serial.print(",");
  Serial.print(angle);
  Serial.print("\n");
  // if (angle > 255) {
  //   angle=255;
  // }else if (angle < -255) {
  //   angle=-255;
  // }
 
  if (angle > 0) {
    OCR2B = mtrSpd - abs(angle);
    OCR1A = mtrSpd - abs(angle);
    OCR2A = mtrSpd + abs(angle);
    OCR1B = mtrSpd + abs(angle);

  } else if (angle < 0) {
    OCR2B= mtrSpd + abs(angle);
    OCR1A = mtrSpd + abs(angle);
    OCR2A = mtrSpd - abs(angle);
    OCR1B = mtrSpd - abs(angle);
  }
  
  last_error = error;
  delay(10); 
}
2

Please describe the problem (behavior of the robot).

Which tuning method did you use to select the K values?

3

The problem is that the robot runs on a "straight" line which is not that straight and if I voluntarily rotate it to simulate an obstacle up to like +- 10 degrees from 0 it returns to the 0 line, when instead I go out of that range which I don't even know if it's +-10 degrees he starts to go in a straight line towards where I turned him and with the engines at maximum.....

4

You need to tune the PID constants. Search the web for "pid tuning" to learn how.

5

I personally selected the K values ​​to make it work as best as possible (attempts)

6

I imagine that your K-parameter selection process could be improved.

7

yeah ok but if i change value of the K it doesen't work....is the program right???

8

There is nothing obviously wrong with the code, but you do not seem to understand how to choose K values.

Try the well designed and tested Arduino PID library, after going through this tutorial:
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

9

ok now I'll see....but one last question before going, is the pid for a 4-wheeled robot one or are they two, one on each side?

10

One PID loop is required for each individual variable to be controlled.

If the steering angle is the only controlled variable, one PID loop is required.

11

and with the output i have to make my program for like motors on left and motors on right?

12

It is up to you to decide how to physically steer the robot. The following might be OK, but "angle" is a poor choice for the variable name, if controlling motor power.

  if (angle > 0) {
    OCR2B = mtrSpd - abs(angle);
    OCR1A = mtrSpd - abs(angle);
    OCR2A = mtrSpd + abs(angle);
    OCR1B = mtrSpd + abs(angle);

  } else if (angle < 0) {
    OCR2B= mtrSpd + abs(angle);
    OCR1A = mtrSpd + abs(angle);
    OCR2A = mtrSpd - abs(angle);
    OCR1B = mtrSpd - abs(angle);
  }
13

when i need to code this part i need to do like this

 if (angle > 0) {
    OCR2B = mtrSpd - abs(angle);
    OCR1A = mtrSpd - abs(angle);
    OCR2A = mtrSpd + abs(angle);
    OCR1B = mtrSpd + abs(angle);

  } else if (angle < 0) {
    OCR2B= mtrSpd + abs(angle);
    OCR1A = mtrSpd + abs(angle);
    OCR2A = mtrSpd - abs(angle);
    OCR1B = mtrSpd - abs(angle);
  }```

or i need to take the rotation from bno055 and send output of the pid to the motor???
14

Do more reading on PID control. There are countless tutorials posted on line, including the one linked in post #8.

15

i'm curious, what is being controlled here? is angle the direction of the robot or the angle of rotation of a wheel?

16

Just the rotation of the robot