Line Follower Spins out of Control

Other Hardware Robotics
1

I am trying to make a line follower robot and this is the code I have been using

#include <Arduino.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <TimerOne.h>
#include <QTRSensors.h>

#define NUM_MOTORS 2
#define SensorCount 5
#define WHEEL_RADIUS 0.035
#define DT 0.040
#define WHEEL_DIST 0.125
#define NUM_SAMPLES 4

QTRSensors qtr;
volatile uint16_t position;
uint16_t sensorValues[SensorCount];

//PID CONTROL
float Kp = 2.2; //2.2
float Ki = 0;
float Kd = 4; //4
volatile int P;
volatile int I;
volatile int D;

volatile int motorspeed;

volatile int lastError;
volatile int error;

int maxspeeda = 750;
int maxspeedb = 750;
int basespeeda = 400;
int basespeedb = 400;


const int encoder0A_pin = 2;  // Motor1 encoder A input
const int encoder0B_pin = 3;  // Motor1 encoder B input
const int encoder1A_pin = 4;  // Motor2 encoder A input
const int encoder1B_pin = 5;  // Motor2 encoder B input

const int M1_dir_pin = 8;   // Motor1 Dir output
const int M2_dir_pin = 11;  // Motor2 Dir output

const int M1_PWM_pin = 9;   // Motor1 PWM output
const int M2_PWM_pin = 10;  // Motor2 PWM output

volatile int M_PWM_value[NUM_MOTORS];
uint8_t LED_state;

static void set_M1_PWM(volatile int new_PWM)
{
  if (new_PWM >= 0) {
    M_PWM_value[0] = new_PWM;
    digitalWrite(M1_dir_pin, false);
  }else if (new_PWM < 0){
    M_PWM_value[0] = -new_PWM;
    digitalWrite(M1_dir_pin, true);
  }
  Timer1.setPwmDuty(TIMER1_A_PIN, M_PWM_value[0]);
}


static void set_M2_PWM(volatile int new_PWM)
{
  if (new_PWM >= 0) {
      M_PWM_value[1] = new_PWM;
      digitalWrite(M2_dir_pin, false);
  }else if (new_PWM < 0){
      M_PWM_value[1] = -new_PWM;
      digitalWrite(M2_dir_pin, true);
  }
  Timer1.setPwmDuty(TIMER1_B_PIN, M_PWM_value[1]);
}

void calibration() {
  digitalWrite(LED_BUILTIN, HIGH);
  for (uint16_t i = 0; i < 400; i++)
  {
    qtr.calibrate();
  }
  digitalWrite(LED_BUILTIN, LOW);
}

void PID_control() {
  error = 2000 - position;

  P = error;
  I += error;
  D = error - lastError;
  lastError = error;
  motorspeed = P*Kp + D*Kd + I*Ki;

  if (motorspeed > 200 || motorspeed < -200){
    I -= error;
  }
  M_PWM_value[0] = basespeeda - motorspeed;
  M_PWM_value[1] = basespeedb + motorspeed;
  
  if (M_PWM_value[0] > maxspeeda) {
    M_PWM_value[0] = maxspeeda;
  }else if (-M_PWM_value[0] > maxspeeda){
    M_PWM_value[0] = -maxspeeda;
  }

  if (M_PWM_value[1] > maxspeedb) {
    M_PWM_value[1] = maxspeedb;
  }else if (-M_PWM_value[1] > maxspeedb){
    M_PWM_value[1] = -maxspeedb;
  }
}

typedef enum { 
  cm_pwm,
  cm_pid
} control_mode_t;

typedef struct {
  //float v1e, v2e;

  float ve, we;
  float ds, dtheta;
  float rel_s, rel_theta;
  float x, y, theta;
  
  uint8_t state, new_state, prev_state;
  uint32_t tis, tes;

  float dt;
  float v, w;
  float v_req, w_req;

  control_mode_t control_mode;

} robot_t;

unsigned long interval;
unsigned long currentMicros, previousMicros;

volatile uint16_t encoder_delta[NUM_MOTORS];
int odo[NUM_MOTORS];
float v_wheel[NUM_MOTORS];
float v_wheel_ref[NUM_MOTORS];

robot_t robot;

VL53L0X sensor;
float distance, prev_distance;

#pragma GCC push_options
#pragma GCC optimize ("O3")

void timer_interrupt(void){
  byte b, new_state;
  static int8_t encoder_table[16] = {0, 1, -1, 0,  -1, 0, 0, 1,  1, 0, 0, -1,  0, -1, 1, 0};
  static byte encoder_state[2];

  b = PIND;

  new_state = (b >> 2) & 0x03; // Put encoder 0 channels in the lowest bits
  encoder_delta[0] += encoder_table[encoder_state[0] | new_state];
  encoder_state[0] = new_state << 2;

  new_state = (b >> 4) & 0x03; // Put encoder 1 channels in the lowest bits
  encoder_delta[1] += encoder_table[encoder_state[1] | new_state];
  encoder_state[1] = new_state << 2;
}


static void set_motor_PWM(byte motor, volatile int new_PWM) // Motor control
{
  if (motor == 0) set_M1_PWM(new_PWM);
  else if (motor == 1) set_M2_PWM(new_PWM);
}


void set_robot_state(robot_t& robot, uint8_t new_state){
  if (robot.state != new_state) {  // if the state chnanged tis is reset
    robot.prev_state = robot.state;
    robot.state = new_state;
    robot.tes = millis();
    robot.tis = 0;
  }
}

void obstacle_dodge(){
  
}

void setup() {
  pinMode(encoder0A_pin, INPUT_PULLUP);
  pinMode(encoder0B_pin, INPUT_PULLUP);
  pinMode(encoder1A_pin, INPUT_PULLUP);
  pinMode(encoder1B_pin, INPUT_PULLUP);

  pinMode(M1_dir_pin, OUTPUT);
  pinMode(M2_dir_pin, OUTPUT);

  pinMode(M1_PWM_pin, OUTPUT);
  pinMode(M2_PWM_pin, OUTPUT);

  interval = 40UL * 1000UL;
  LED_state = 1;
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LED_state);

  Timer1.attachInterrupt(timer_interrupt);
  Timer1.initialize(40); //uS
  Timer1.pwm(TIMER1_A_PIN, 0);
  Timer1.pwm(TIMER1_B_PIN, 0);
  set_motor_PWM(0, 0);
  set_motor_PWM(1, 0);

  Serial.begin(115200);
  Wire.begin();
  /*
  sensor.setTimeout(500);
  while (!sensor.init()) {
    Serial.println(F("Failed to detect and initialize sensor!"));
    delay(100);
  }*/

  qtr.setTypeAnalog();
  qtr.setSensorPins((const uint8_t[]){1, 2, 3, 6, 7}, SensorCount);
  qtr.setSamplesPerSensor(NUM_SAMPLES);
  calibration();
  
  previousMicros = micros();
  robot.state = 10;
}

void loop() {
  byte i, l;

  if(sensor.readRangeAvailable()) { // distance measurement
    prev_distance = distance;
    distance = sensor.readRangeMillimeters() * 1e-3;
  }

  currentMicros = micros();

  if (currentMicros - previousMicros >= interval) {
    previousMicros = currentMicros;

    LED_state = !LED_state;
    digitalWrite(LED_BUILTIN, LED_state);

    // Read Encoders
    for(i = 0; i < NUM_MOTORS; i++) {
      noInterrupts();
      odo[i] = encoder_delta[i];
      encoder_delta[i] = 0;
      interrupts();
    }

    // Start new distance measure
    sensor.startReadRangeMillimeters();

    for(i = 0; i < NUM_MOTORS; i++) {  //Wheel Speed
      v_wheel[i] =  TWO_PI * WHEEL_RADIUS / (2 * 1920.0 * DT) * odo[i];
    }

    robot.ve = (v_wheel[0] - v_wheel[1]) / 2;
    robot.we = (v_wheel[0] + v_wheel[1]) / WHEEL_DIST;

    robot.ds = robot.ve * DT;
    robot.dtheta = robot.we * DT;

    robot.rel_s += robot.ds;
    robot.rel_theta += robot.dtheta;

    v_wheel_ref[0] = robot.v + robot.w * WHEEL_DIST / 2;
    v_wheel_ref[1] = robot.v - robot.w * WHEEL_DIST / 2;

    position = qtr.readLineBlack(sensorValues);

    if(distance < 0.25){
      robot.state = 1;
    }else{
      robot.state = 10;
    }

    //What to execute in each state
    if (robot.state == 1) {  // The robot is stopped
      M_PWM_value[0] = 0;
      M_PWM_value[1] = 0;    
    }else if (robot.state == 10) {  // Test
      PID_control();   
    }

    for(l = 0; l < NUM_MOTORS; l++) {
      noInterrupts();
      set_motor_PWM(l,  M_PWM_value[l]);
      interrupts();
    }    
  }
}

For some reason, the robot spontaneously starts spinning and it doesn't stop until I reset it. This becomes worse as I increase the values of Kp, Ki or Kd reaching a point where it doesn't even try to follow the line, as soon as it enters the loop, it spins. At first I expected it to be some kind of variable overflow but I printed most of the values that I am using and they are far from their limit. My main suspicion now is that as I increase these constants, the motors can sometimes switch their direction very fast and that might cause some sort of trouble....
I pretty much ran out of ideas as to what could be causing this and possible solutions. I would appreciate if someone could give me some help.

2

There are many tutorials explaining how to determine the various K constants. Google "PID tuning" to find them.

But first, you have to determine whether the signs of input and output are correct. Put in print statements to see the QTR sensor output for deviations from the line, and to determine whether the changes to motor speed make sense and are in the correct direction.

3

My problem isn't related to finding these values, for example the ones I currently have work very well, the problem is that after a certain distance the robot goes out of control and just starts spinning until I reset it. If I try for example Kp = 2.8, the robot doesn't even move, it instantly starts spinning and it no longer responds to the line. As to the signs of the inputs and outputs they are all correct.

4

It would have been wise to make that clear in the first post. There are many other possibilities to explain such behavior, such as program errors, integer math overflow, overloaded motor drivers, inadequate battery power, etc.

More details are required to diagnose such problems. Follow the "How to get the best out of the forum" link at the head of every topic.

5

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