PID control

hi
i want to add PID control to my motor and ulteasonic sensor to make the it stop at exactly 50cm.
here is the code i use to control my motor and sensor.

#define CW  0
#define CCW 1

#define MOTOR_A 0
#define MOTOR_B 1

const byte PWMA = 3;  // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13;

void setup() {
  // put your setup code here, to run once:
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);
  
  // Initialize all pins as low:
  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(DIRA, LOW);
  digitalWrite(DIRB, LOW);
}

void loop() {
  // put your main code here, to run repeatedly:
  driveArdumoto(MOTOR_A, CCW, 255); 
  driveArdumoto(MOTOR_B, CW, 255);
}
void driveArdumoto(byte motor, byte dir, byte spd)
{
  if (motor == MOTOR_A)
  {
    digitalWrite(DIRA, dir);
    analogWrite(PWMA, spd);
  }
  else if (motor == MOTOR_B)
  {
    digitalWrite(DIRB, dir);
    analogWrite(PWMB, spd);
  } 
}
//Feel free to use this code.

//Please be respectful by acknowledging the author in the code if you use or modify it.

//Author: Bruce Allen

//Date: 23/07/09



//Analog pin 1 for reading in the analog voltage from the MaxSonar device.

//This variable is a constant because the pin will not change throughout execution of this code.

const int anPin = 15;



//variables needed to store values

long anVolt, inches, cm;

int sum = 0; //Create sum variable so it can be averaged

int avgrange = 60; //Quantity of values to average (sample size)



void setup()

{

  //This opens up a serial connection to shoot the results back to the PC console

  Serial.begin(9600);

}



void loop()

{

  //MaxSonar Analog reads are known to be very sensitive. See the Arduino forum for more information.

  //A simple fix is to average out a sample of n readings to get a more consistent reading.

  //Even with averaging I still find it to be less accurate than the PW method.

  //This loop gets 60 reads and averages them



  for (int i = 0; i < avgrange ; i++)

  {

    //Used to read in the analog voltage output that is being sent by the MaxSonar device.

    //Scale factor is (Vcc/512) per inch. A 5V supply yields ~9.8mV/in

    //Arduino analog pin goes from 0 to 1024, so the value has to be divided by 2 to get the actual inches

    anVolt = analogRead(anPin) / 2;

    sum += anVolt;

    delay(10);

  }



  inches = sum / avgrange;

  cm = inches * 2.54;

  Serial.print(inches);

  Serial.print("in, ");

  Serial.print(cm);

  Serial.print("cm");

  Serial.println();



  //reset sample total

  sum = 0;



  delay(50);

}

HOW CAN I ADD PID CONTROL?
I only see automatic mode on the example given.
thanks

Hello,

To make a PID with your sensor, you first need to know the value it takes at that distance in an analogous way, ie for example at 50m 820 units in analog conversion.

It is worth once you have that, and go through a cycle you read it again and suppose that due to natural noise, or white noise, we read 840. The difference is 840-800 ie 40. Now we know that our error is of 40 units and we store it in a variable. We re-read it again and note that it reads 815, so 815-840 is -25, ie the current error is -25 and the previous 40.

Well the algorithm of the PID is a costant of value proportional to the input KP, by the current error. To this we add KI, integral constant by the summation of the error, ie accumulate in a variable all the errors. And finally add KD constant Derivative and multiply by the current error less the previous error.

The value we get from PID will be restrained at the entrance of the engines.

It would be roughly as follows.

-Assign values ​​to KP, KI, KD and value. -Read sensor. -errorProp = Current reading - previous reading. - Store the read sensor value in a variable as read above. -errorInetgral = + errorProp. -PID = KP * errorProp + KI * integral error + KD * (errorProp-errorDeriv). ErrorDeriv = errorProp. -Motor1 = value - PID. -Motor2 = value + PID.

You may have to change + by - depending on the rotation of your engines.

I hope it helps you.

Greetings,

For ultrasonic sensor there are many ways to measure and i know how to convert it to cm. but the thing is when i input the ultrasonic sensor distance and set the setpoint, the output of PID is very weired seen from serial monitor and i dont know how to use them.
i understand ur concept but i am not sure how to add PID in my program.
below is the program i write.

#include <PID_v1.h>
const int pwPin = 52;

long pulse, inches, cm;
double Setpoint, Input, Output;

double Kp=2, Ki=5, Kd=1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd,REVERSE);
#define CW  0
#define CCW 1

#define MOTOR_A 0
#define MOTOR_B 1

const byte PWMA = 3;  // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13;



void setup()

{
  Serial.begin(9600);
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);
  
  // Initialize all pins as low:
  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(DIRA, LOW);
  digitalWrite(DIRB, LOW);
}



void loop()

{

  pinMode(pwPin, INPUT);



  pulse = pulseIn(pwPin, HIGH);

  inches = pulse / 147;

  cm = inches * 2.54;
  Input=cm;
  Setpoint=30;
  myPID.SetMode(AUTOMATIC);
  myPID.Compute();

  if(Output<120)
  {
      driveArdumoto(MOTOR_A, CCW, 80); 
      driveArdumoto(MOTOR_B, CW, 80);
  }
  else
  {
      driveArdumoto(MOTOR_A, CW, 80); 
      driveArdumoto(MOTOR_B, CCW, 80);
  }


  Serial.print(cm);
  Serial.print("cm");
  Serial.print(Output);
  Serial.println("PID");



  delay(500);

}

void driveArdumoto(byte motor, byte dir, byte spd)
{
  if (motor == MOTOR_A)
  {
    digitalWrite(DIRA, dir);
    analogWrite(PWMA, spd);
  }
  else if (motor == MOTOR_B)
  {
    digitalWrite(DIRB, dir);
    analogWrite(PWMB, spd);
  } 
}

Because you do not try to work without a library, maybe if you follow what I said above it will be easier to do the program.

He thinks that the output variables are the PWM value of the motors, which will vary depending on the parameter you read from the ultrasonic sensor. Also it is better that you work with the analog value and not with the converted to cm since you lose resolution and your PID would become more unstable.

Try to do it as I tell you and if you have problems you tell me.

Part of your problem is likely that you are using Output only as Forward/Backward signal.

 if(Output<120)
  {
      driveArdumoto(MOTOR_A, CCW, 80); 
      driveArdumoto(MOTOR_B, CW, 80);
  }
  else
  {
      driveArdumoto(MOTOR_A, CW, 80); 
      driveArdumoto(MOTOR_B, CCW, 80);
  }

I recommend you use setOutputLimits() to set the Output range to -255,+255. Then you can use Output for both direction and speed:

 if(Output<0)
  {
      driveArdumoto(MOTOR_A, CCW, -Output); 
      driveArdumoto(MOTOR_B, CW, -Output);
  }
  else
  {
      driveArdumoto(MOTOR_A, CW, Output); 
      driveArdumoto(MOTOR_B, CCW, Output);
  }

First thing, i am using maxsonar ultrasonic sensor to measure the distance, i have tried pwm and analoge method but both didnt work well, while pwm is slightly better than analoge, thats why i use pwm. if there is other good method please tell me.
For PID, i am still testing and i have written a program to test the output by input a increasing set of number but i dont know how to add that inside my motor so that it will stop at 50cm. any suggestions?

1035700564:
any suggestions?

johnwasser:
I recommend you use setOutputLimits() to set the Output range to -255,+255. Then you can use Output for both direction and speed:

 if(Output<0)

{
     driveArdumoto(MOTOR_A, CCW, -Output);
     driveArdumoto(MOTOR_B, CW, -Output);
 }
 else
 {
     driveArdumoto(MOTOR_A, CW, Output);
     driveArdumoto(MOTOR_B, CCW, Output);
 }