How to control the speed of a stepper motor using PID control and ultrasonic sensor?

Question

Little backstory: I'm currently doing this project that deals with using two cars, called block A and B, which block B has to maintain a distance of 10 cm from block A using PID, PD, PI, or P. I'm using a PID. Block B uses an Arduino whereas Block A is controlled by the user by hand. Block B uses a unipolar stepper motor as the actuator and an ultrasonic sensor to sense the distance. My professor wants the motor to move in both directions and have varying speeds (slow, medium, and fast). I'm using brett's PID since I have used it before in my previous labs.

Problem: I have an issue with how to create varying speeds for block B like intuitively I know that I want the B should move for example, fast if the car is greater than 20 cm, medium if the car is between 20cm and 14cm, and slow if it's between 14cm and 10cm. But I just can't use the input value retrieved from the sensor directly to control the motor as it would make it an open system. So I used the error retrieved from Brett's PID code to control the stepper motor. So far, I have gotten the directions to work by setting the myPID.SetOutputLimits(-800,800);. But as it tried to use the error to control the speed it would be impossible because the error always fluctuates at a specific distance. For example at 12cm, I would get either 800 or around 300. I'm currently confused about how to implement control of the speed of my stepper motor through PID and any help regarding this issue will be appreciated.

Code: Code was through Arduino IDE.

#include "SR04.h"
#include <Stepper.h>
#include <PID_v1.h>

#define TRIG_PIN 7
#define ECHO_PIN 6

//intialization of Ultrasonic sensor
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long s; 

//intializing motor variables
int stepsPerRevolution = 2048;
int motorSpeed = 6;
Stepper myStepper (stepsPerRevolution, 8, 10, 9, 11);

//Declared PID variables
double Setpoint = 10; //desired temp value
double Input;    //thermsitor
double Output;   //DC motor
double Error;

//defined variables for PID parameters
double Kp=100, Ki=10, Kd=1;

//PID equation
PID myPID(&Input, &Output, &Setpoint, Kp, Kd, Ki, REVERSE);


void setup(){
  Serial.begin(9600);

  //setting PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-800,800);

  //speed intialized
  myStepper.setSpeed(motorSpeed);
 

}

void loop(){
  s=sr04.Distance();
  Input = s;
  myPID.Compute();
  Error = Input - Setpoint;
  //Serial.print(Input);
  //Serial.print(",");
  //Serial.println(Setpoint);
  Serial.println(Output);
  //Serial.print(",");
  //Serial.println(Error);
  Error = Output; 
  
//Away from Block B
 if (0<Error<800){
    myStepper.setSpeed(motorSpeed);
    myStepper.step(-300);
  } //slow speed
  
   if (Error>=800){
    myStepper.setSpeed(motorSpeed*2);
    myStepper.step(-128);
  } //fast speed

//Towards Block B
  if (-800<Error<0) {
    myStepper.setSpeed(motorSpeed);
    myStepper.step(128);
  } //slow speed
  
  if (Error<=-800) {
    myStepper.setSpeed(motorSpeed*2);
    myStepper.step(128);
  }//Fast speed
 }
  

Answer 1

What you need to do is calcuulate how much you need to change your current speed to minimize the error in distance.

Your calculation for error is not in the right place.

void loop()
{

    long s=sr04.Distance();
    Input = s;             // using global variables to pass values to your PID 
                           // is not a good idea.  Use function parameters instead.
                           // You are storing a 32 bit value in a 16 bit variable!!!
                           // That's only the start of your problems.  
    myPID.Compute();
    Error = Input - Setpoint; // 

Since we're starting with a major design flaw, I'll have to assume you'll fix that and change your PID code to accept and compute long integers both as input value as a function parameter, and as the type of its return value..

What you want to do is compute the PID from the error in distance from your set point, and then modulate the current speed accordingly. PIDs work best when used directly, using 7 speeds (1 stopped, 3 forward/3 backwards) is possible, but I don't think it'll give better results, I'll leave the exercise to you.

I haven't tried this, I don't have any cars on hand. This is a skeletoon of how I'd go about it. Tuning the PID should be what takes you the longest.

//...

// speeds are in RPMs.
long curSpeed = 0;
const long MAX_SPEED = XXX;  // whatever you max speed is for your car.
const long MIN_NEG_SPEED = -XXX;  // whatever you max speed is for your car going reverse.
const long MIN_SPEED = XXX;  // below this absolute speed, we're stopped. 
const int SLICE_TIME = 10;   // time between readings and adjustments, in ms.
                             // you'll need to adjust this according to you minimum speed, and steps per turn. 
const long STEPS_PER_TURN = 200;  // change to whatever you steps/turn value is.

// you'll need to limit the output of your PID to match the acceleration your
// motors can handle for your particular car.

// returns the number of steps to run for our slice time.  
long Steps(int speed)
{
    if (-MIN_SPEED <= speed && speed <= MIN_SPEED)
        return 0;

    // compute number of steps for our slice time.
    // choose slice time and minimum speed wisely!!
    long steps = (SLICE_TIME * (speed * STEPS_PER_TURN)) / (60000L);
    
    // for very low speeds.  I've added this, because I'm unsure of the 
    // time domain behaviour of stepper library with less than 2 steps
    if (-1 <= steps && steps <= 1)
    {
        if (speed < 0)
            return -2;
        else
            return 2;
    } 
    return int(steps);
}

void loop() 
{
    // You may want to filter the sr04 readings with a median of 5
    // filter to limit input noise.   

    // You want to keep the car at a distance of 'set_point'
    // from the leading car.  distance_error is the error you want to
    // minimize to zero by using the PID, and that's what should be
    // the PID input.
    //
    // The way this works.  We are rolling at speed curSpeed, we
    // measure the error in distance from our set_point, feed that 
    // to the PID, then accelerate or decelerate by subtracting
    // the output of the PID from the current speed. 
    //    
    // Note: you can add or subtract the PID to/from the current speed,
    // the sign of the PID depends on you coefficients and sensor. 
    // I've used subtraction here because that's how you express 
    // negative feedback mathematically.  In real life, we'll use what
    // best fits our needs.  Usually it's the operation that makes P
    // positive.

    long distance_error = sr04.Distance() - setPoint;

    long pid_out = myPID.Compute(distance_error);

    // increment or decrement the current speed to try and reduce the error.
    long speed = curSpeed - pid_out;  // As usual, PID coefficients do matter 
                                     // for this to work well.

    if (speed > MAX_SPEED)
        speed = MAX_SPEED;
    if (speed < MIN_NEG_SPEED)
        speed = MIN_NEG_SPEED;

    curSpeed = speed;
    if (speed < 0)
        speed = -speed;

    myStepper.setSpeed(speed);     // modulate speed
    int steps = Steps(curSpeed);
    if (steps)
        myStepper.step(steps);     // keep rolling.
}

I haven't tried to compile it either, so this may not compile as is. But most of the tricks and traps are covered, and this should give you a head start, if you want to go the PID route. But I think your professor will really wonder where that one came from :) Still, you should try and make it run, for fun.

The other way, without a PID, and using set speeds is much more straightforward. It may also be closer to what the is required by the exercise. The distance between cars will vary a bit more, of course. And it does not use a PID at all.

const int MAX_SPEED = 3;
int speed = 0;    // value range is [-MAX_SPEED, +MAX_SPEED]  

long RPMS[MAX_SPEED + 1] = { 0, 200, 400, 800 }; // in RPMs, assuming average speed will be around 400, in this case.
// For 3 speeds, the difference between speeds cannot be higher than max acceleration.  
// You can add as many speeds as desired.  More speeds = more precision.

const long STEPS_PER_TURN = 200;  // change to whatever you steps/turn value is.  MUST be 200 or more.
const int STEPS = STEPS_PER_TURN / 100;  // 3.6° between speed adjustment.
                                         // it is very small right now, so 
                                         // you will want to play with this value.

// this threshold gives some control over aceleration.
// and 'hardness' of distance tracking.  
const long THRESHOLD = 0;

void loop()
{
    // get the error in distance.
    long distance_error = sr04.Distance() - setPoint;

    // modulate speed.  
    if (distance_error > THRESHOLD) 
        ++speed;
    if (distance_error < -THRESHOLD)
        --speed;

    if (speed > MAX_SPEED)
        speed = MAX_SPEED;
    if (speed < -MAX_SPEED)
        speed = -MAX_SPEED;

    long rpm = RPMS[(speed < 0) : -speed : speed];
    if (rpm)
    {
        myStepper.setSpeed(rpm);
        myStepper.setSpeed((speed < 0) ? -STEPS : STEPS)
    }
}

For this code, you must choose speeds and STEPS value that will give you an acceleration without misssed steps.