Go Down

Topic: Help with PID in arduino (Read 675 times) previous topic - next topic

Ahmer

I want to make a autonomous robotic car(it's just basic now) using arduino by implementing PID on distance as a input by ultrasonic sensor. I am just using one ultrasonic sensor which will control speed of my car in forward direction. I want it so that greater the distance, greater the PWM supplied to motor. I have written code but it is not responding smoothly.

I have set the set point as 15cm and I want PWM to be 0 when distance is also 15cm and increase accordingly with increase in distance. I have set the range of car to 400cm.

I checked it and PWM was 255 for distance greater than 50cm and for less than it was 0 means it is responding smoothly.

Do identify bugs and suggest if any improvement is possible.

Code: [Select]
#include <Ultrasonic.h>
    #define mb1 8
    #define mb2 9
    #define mf1 10
    #define mf2 11
    #define en1 5
    #define en2 6
    #define trigPin 12
    #define echoPin 13
    int Output;
    int Input;
    long duration;
    int Setpoint = 15;
    int Error[10];
    long Accumulator=0;
    int PTerm;
    int ITerm;
    int DTerm;
    byte Divider;

    Ultrasonic ultrasonic(trigPin, echoPin);

    void setup() {
        Serial.begin(9600);
        pinMode(mb1, OUTPUT);
        pinMode(mb2, OUTPUT);
        pinMode(mf1, OUTPUT);
        pinMode(mf2, OUTPUT);
        pinMode(en1, OUTPUT);
        pinMode(en2, OUTPUT);
        pinMode(trigPin, OUTPUT);
        pinMode(echoPin, INPUT);
    }

    void forward(byte Output)
    {
    analogWrite(en1,Output);
    analogWrite(en2,0);
    digitalWrite(mb1, HIGH);
    digitalWrite(mb2, LOW);
    digitalWrite(mf1, LOW);
    digitalWrite(mf2, LOW);
    }

    void GetError()
    {
      byte i = 0;
      for(i=9;i>0;i--)
        Error[i] = Error[i-1];
      // load new error into top array spot 
      Error[0] = Setpoint-Input;
    }

    void CalculatePID()
    {
    // Set constants here
      PTerm = 500;
      ITerm = 10;
      DTerm = 4;
      Divider = 2;

      Output = abs(Error[0])*PTerm;     // start with proportional gain
      Accumulator += abs(Error[0]);  // accumulator is sum of errors
      Output += ITerm*Accumulator; // add integral gain and error accumulation
      Output += DTerm*(abs(Error[0])-abs(Error[9])); // differential gain comes next
      Output = Output>>Divider;
      if (Accumulator >1000) Accumulator = 1000;
      if (Accumulator <-1000) Accumulator = -1000;
      if (Output <255 & Output >0){
         Output = Output;
       }
      else if (Output <= 0) {
         Output = 0;
       }
      else if (Output >= 255 ) {
         Output = 255;
       }

    }

    void loop() // run over and over
    {

      Input = ultrasonic.distanceRead();
      if (Input >400) {

      }
      else {
      GetError();       // Get position error
      CalculatePID();   // Calculate the PID output from the error
      forward(Output);
      Serial.print(Input);
      Serial.print("cm\t");
      Serial.print(Output);
      Serial.print("PWM");
      Serial.println("");
      delay(1000);
    }
    }

PaulS

Code: [Select]
    int PTerm;
    int ITerm;
    int DTerm;

Why are these ints?

Code: [Select]
      if (Output <255 & Output >0){
         Output = Output;
       }

Assigning the contents of a variable to itself is pointless. Using & in place of && is wrong.

There IS a PID library. Use it, instead of (incorrectly) trying to roll your own.
The art of getting good answers lies in asking good questions.

Go Up