PID ball stabilization

Projects Programming
1

Hi guys. Can someone be able to help me with this school project? I have a project in which I would to stabilizce ball in the middle of the plane between two such guide rods by tuning the parameters of a PID controller (algorithm). I use stepper motor NEMA 17 to control the platform and stepper motor driver TMC2209. I use two laser sensor VL53L0X V2, each on one side, to measure the ball distance. I need help with programming. Any advice for help will be appreciated. Thank you. :slight_smile:

code sample what I have written so far:

#include "Adafruit_VL53L0X.h"

#define LOX1_ADDRESS 0x37
#define LOX2_ADDRESS 0x38

#define SHT_LOX1 12
#define SHT_LOX2 11

Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();

VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;

int distanceLeft, distanceRight;

int EN_pin = 10;         
int DIR_pin = 3;        
int STEP_pin = 4; 

int MS1 = 9;
int MS2 = 8;

float Kp = 8;
float Ki = 0.2;
float Kd = 3100;

float proportionalTerm = 0;
float integralTerm = 0;
float derivativeTerm = 0;

float PID_output = 0;

float error = 0;
float previousError = 0;

float center = 0;
float inputDistance = 0;

long currentTime = 0;
long previousTime = 0;
float timeDifference = 0;

void setID() {
  // all reset
  digitalWrite(SHT_LOX1, LOW);    
  digitalWrite(SHT_LOX2, LOW);
  delay(10);
  // all unreset
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  // activating LOX1 and resetting LOX2
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, LOW);

  if(!lox1.begin(LOX1_ADDRESS)) {
    Serial.println(F("Failed to boot first VL53L0X"));
    while(1);
  }
  delay(10);

  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  if(!lox2.begin(LOX2_ADDRESS)) {
    Serial.println(F("Failed to boot second VL53L0X"));
    while(1);
  }
}

// Function to read distances from both sensors
void read_dual_sensors() {

  // Measure distance from sensor LOX1
  lox1.rangingTest(&measure1, false); 

  // Measure distance from sensor LOX2
  lox2.rangingTest(&measure2, false); 

  Serial.print(F("1: "));
  if(measure1.RangeStatus != 4) {     
    Serial.print(measure1.RangeMilliMeter);

    distanceLeft = measure1.RangeMilliMeter;

  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.print(F(" "));

  Serial.print(F("2: "));
  if(measure2.RangeStatus != 4) {
    Serial.print(measure2.RangeMilliMeter);

    distanceRight = measure2.RangeMilliMeter;

  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.println();
}

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

  while (!Serial) { delay(1); }

  pinMode(SHT_LOX1, OUTPUT);
  pinMode(SHT_LOX2, OUTPUT);

  Serial.println(F("Shutdown pins initialized..."));

  digitalWrite(SHT_LOX1, LOW);
  digitalWrite(SHT_LOX2, LOW);

  Serial.println(F("Both in reset mode...(pins are low)"));
  
  Serial.println(F("Starting..."));
  setID();

  pinMode(EN_pin, OUTPUT);
  pinMode(DIR_pin, OUTPUT);
  pinMode(STEP_pin, OUTPUT);

  digitalWrite(EN_pin, LOW);
  digitalWrite(DIR_pin, HIGH);
  digitalWrite(MS1, HIGH);
  digitalWrite(MS2, HIGH);
}

void loop() {
   
  read_dual_sensors();
  
  if (distanceLeft != -1 && distanceRight != -1)
  {
      currentTime = millis();

      timeDifference = currentTime - previousTime;

      if(distanceLeft < 200) error = distanceLeft - 200;
      else error = 200 - distanceRight;
   
      proportionalTerm = Kp * error;

      derivativeTerm = Kd * ((error - previousError) / timeDifference);
        
      if (-30 < error && error < -5 || 5 < error && error < 30 )
        {
          integralTerm = integralTerm + (Ki * error * timeDifference);
        }
        else
        {
          integralTerm = 0;
        }

      PID_output = proportionalTerm + integralTerm + derivativeTerm;

      previousError = error;

      previousTime = currentTime;
    
      Serial.println(PID_output);
  }

  delay(100);
}
2

What works, what does not?

3

I would need to add code to control a stepper motor based on calculating PID

4

The PID algorithm calculations should work. I just don't know how to synchronize it with the stepper motor.

5

Take a look how it's done here:

6

Thank you

7

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.