I am working on control system project. I bought a Zumo 32U4. It has built in encoders. The project is divided into three parts:
a) Move in a straight line.
b) Slows down as obstacle approaches.
c) Obstacle Avoidance.

This means that I will have to implement three PID controllers for this project. I am still on the first part. I have implemented the following code:

#include <PID_v1.h>
#include <Wire.h>
#include <Zumo32U4.h>
#include <VL53L0X.h>


double Setpoint, Input, Output;
double Initial = 150;
double Kp=1.1, Ki=0.395, Kd=0.004;
//Kp = 100 Ki = 300

PID DB(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

Zumo32U4Encoders encoders;
Zumo32U4LCD lcd;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;

const char encoderErrorLeft[] PROGMEM = "!<c2";
const char encoderErrorRight[] PROGMEM = "!<e2";
char report[80];
int State = 0;

void setup()

  Setpoint = 0;
  // Delay so that the robot does not move away while the user is
  // still touching it.


  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).

void loop()
  int16_t countsLeft = encoders.getCountsLeft();
  int16_t countsRight = encoders.getCountsRight();


  static uint8_t lastDisplayTime;
  static uint8_t displayErrorLeftCount = 0;
  static uint8_t displayErrorRightCount = 0;

  if ((uint8_t)(millis() - lastDisplayTime) >= 100)
    lastDisplayTime = millis();

    int16_t countsLeft = encoders.getCountsLeft();
    int16_t countsRight = encoders.getCountsRight();

    bool errorLeft = encoders.checkErrorLeft();
    bool errorRight = encoders.checkErrorRight();

        // An error occurred on the left encoder channel.
        // Display it on the LCD for the next 10 iterations and
        // also beep.
        displayErrorLeftCount = 10;

        // An error occurred on the left encoder channel.
        // Display it on the LCD for the next 10 iterations and
        // also beep.
        displayErrorRightCount = 10;

    // Update the LCD with encoder counts and error info.
    lcd.gotoXY(0, 1);

    if (displayErrorLeftCount)
        // Show an exclamation point on the first line to
        // indicate an error from the left encoder.
        lcd.gotoXY(7, 0);

    if (displayErrorRightCount)
        // Show an exclamation point on the second line to
        // indicate an error from the left encoder.
        lcd.gotoXY(7, 1);

    // Send the information to the serial monitor also.
    snprintf_P(report, sizeof(report),
    PSTR("%6d %6d %1d %1d"),
    countsLeft, countsRight, errorLeft, errorRight);

    Input = countsRight - countsLeft;

My Understanding: I basically grabbed code (may not understand some of it) from various examples and worked with it. When I turned it on, it doesn't move in a straight line. This tells me that the K values are incorrect. I essentially treat one of the motor's speed as input. I subtract the right motor's speed with the left, pass it through the PID and take that output as input for the right motor. The set point value is 0.
I'd like some help as to how to determine the values of K (or correct any of my mistakes). Any help with the other parts of the project will be appreciated.

Zumo 32U4 Library: http://pololu.github.io/zumo-32u4-arduino-library/
Arduino PID Library: https://playground.arduino.cc/Code/PIDLibrary/

  • 1
    \$\begingroup\$ I assume this is for school. Why do the three tasks imply three PID controllers? If you're controlling the right motor to follow the left, what controls the left motor? Why is that mode of control correct for traveling in a straight line? \$\endgroup\$ – TimWescott May 18 at 15:52
  • \$\begingroup\$ Before you choose Kp,Ki,Kd's. Determine the transfer functions of each input and output. Current controls acceleration is there is current sensing and Voltage controls speed which reaches the limit pretty quickly. There will be some hysteresis error due to startup power needed to move. Then decide on a stable acceleration rate to maximum velocity and compute the error between whatever you choose as target or target avoidance whether it is position, direction, velocity, acceleration. and what to do when error saturates or sensor feedback is null. This is 1st step before Ki and Kd \$\endgroup\$ – Tony Stewart Sunnyskyguy EE75 May 18 at 17:08
  • \$\begingroup\$ Delay may cause instability in any loop variable, so this is where the k factors are then tuned by whatever method you choose such that any result does not saturate the forcing function. This is why controlled acceleration is useful so you have minimal error on velocity feedback and direction. The 1 spin around stores the sensor peak values when sensor feedback is either null or error is excessive \$\endgroup\$ – Tony Stewart Sunnyskyguy EE75 May 18 at 17:12
  • \$\begingroup\$ I see this board has no current sensing so you have to measure the time to reach full velocity or direction angle change to make some assumptions on time delays in the block diagram for your error rate of change with full brake or reverse power, however you choose to steer. But if you dither back and forth full power, something will overheat at 10x the power being used. So this is a compromise for ramping speed . A dual loop for each motor Voltage and current or speed and acceleration gives the best stable results. Without this, funky things can happen which makes PID settings more critical. \$\endgroup\$ – Tony Stewart Sunnyskyguy EE75 May 18 at 17:31
  • \$\begingroup\$ In control theory, a 1st order proportional loop is inherently stable ie. acceleration error or velocity error. But with only max velocity control with Voltage, you now have at least a 2nd order system which has the properties of damping factor, settling time and hystereis error that you are trying to optimize to avoid funky problems \$\endgroup\$ – Tony Stewart Sunnyskyguy EE75 May 18 at 17:33

You should start with an overall block diagram. Something like this:

enter image description here

Dotted line sections from https://bharat-robotics.github.io/blog/dc-motor-speed-control/ figure 5.

You probably want 2 PID controllers, one for each motor. The 2 right blocks are mostly, or all, handled by the PID code. This is just a start, there are a lot more details to worry about.

Your behavior would be something like:

if no obstacles then
   speedRight = NORMAL_SPEED;
   speedLeft = NORMAL_SPEED;
   reduce speed in one or both motors ...

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.