Continuous Servo Control for ESP32

This article is for personal use to record the way to do servo control "Parallax Feedback 360° High Speed Servo" in ESP32 microcontrollers.

What is Continuous Servo Motor?

Servo motors are closed-loop systems that usually use feedback loops to control position. Because of its high torque compared to its size and ease of use, it is often used for robotics applications. I also frequently use servo motors for simple mechanisms.

Continuous servo motors also use feedback loops to control the motor. However, it's not a position control but more of a speed control. It also does not have the angle limit that most servo motors have. As a result, it is suitable for use as the primary motor in small wheeled robots.

If we are just going to do speed control, we will probably just measure the motor speed and use a simple PWM signal to control it. However, in this article, we are going to talk about accurately position controlling a continuous servo motor using an encoder.

ESP32 Servo Control

As its name implies, ESP32 is a 32-bit MCU that also has a Wi-Fi module built into it. It's much faster than other MCUs like Arduino but has its own Arduino library that works differently with others. And servo control is the one that differs from other MCUs.

LEDC and MCPWM are libraries for ESP32 motor control. Experts say that MCPWM is better suited to high-precision motor control since it makes less noise. However, LEDC is simpler and works well with small, cheap motors. The below is the code for the servo, which works similarly with Arduino's Servo.h library.

#ifndef servo_esp_h
#define servo_esp_h

// You can increase the resolution of PWM
// however that decreases the maximum frequency you can use
#define LEDC_RESOLUTION_BITS 12
#define LEDC_RESOLUTION ((1<<LEDC_RESOLUTION_BITS)-1)

class Servo
{
  public:
    void setChannel(int channel){
      LEDC_CHANNEL = channel;
    }

    void setPeriodHertz(int freq){
      LEDC_FREQ_HZ = freq;
    }

    void attach(int outPin, int min_duty, int max_duty){
      ledcSetup(LEDC_CHANNEL, LEDC_FREQ_HZ, LEDC_RESOLUTION_BITS);
      ledcAttachPin(outPin, LEDC_CHANNEL);
      MIN_DUTY = min_duty;
      MAX_DUTY = max_duty;
    }

    void write(int val){
      // 50 Hz -> 20000 us
      DUTY = LEDC_RESOLUTION * map(val, 0, 180, MIN_DUTY, MAX_DUTY) / 20000;
      ledcWrite(LEDC_CHANNEL, DUTY);
    }

  private:
    int LEDC_CHANNEL = 0;
    int LEDC_FREQ_HZ = 50;
    int DUTY = 0;
    // min, max duty cycles depend on the servo specification
    int MIN_DUTY = 1000; // 1 ms
    int MAX_DUTY = 2000; // 2 ms
};

#endif

There are more things you can control when you use an LEDC library. You can change the frequency and channels. For example, you can run the servo motor at 100 Hz rather than 50 Hz, the usual frequency requirement for servo motors, which also works and gives better speed. And you can change channels so that you can send the PWM signals instantly to any pin in the ESP32. However, if you want to make different servo motors run differently, you have to use different channels for each of them, which is 16 channels maximum. To improve the library, you can add a detach function and initializer, but, in my case, those were not needed.

PID Control Using an Encoder

For a position control, there are three things that you need: a PID controller, angle readings, and a rotation counter.

PID Controller

For the PID controller, you can just use this Arduino library. However, since the motor has to be agile, I recommend you simplify that library by using float (4 bytes) instead of double (8 bytes) and deleting unnecessary functions. This also helps reduce the ESP32 compilation time, which is natively way longer than Arduino boards. Also, be careful about the negative D parameter in the library. It somehow works well, but is not mathematically well described.

Angle Readings

I used a "Parallax Feedback 360° High Speed Servo", which has a hall effect position sensor. To read angles from the hall effect sensor, we have to measure the duty cycle of the signal using interrupts. The interrupt function looks like below.

unsigned long _dc;
volatile unsigned long _pulseInTimeBegin = micros();
volatile unsigned long _pulseInTimeEnd = micros();

void signalInterrupt() {
  if (digitalRead(_feedPin) == HIGH) {
    _pulseInTimeBegin = micros();
  }
  else {
    _pulseInTimeEnd = micros();
    unsigned long temp = _pulseInTimeEnd - _pulseInTimeBegin;
    if ((temp > 0) && (temp < 1500)){_dc = temp;}
  }
}

It is safer to use volatile variables for interrupt functions. And we ignore duty cycles smaller than 0 ms and bigger than 1500 ms since they are considered to be noises.

 int _dcMin = 30;
 int _dcMax = 1071;
 int _unitsFC = 360;
 float _theta;

_theta = (_unitsFC - 1) - ((_dc - _dcMin) * _unitsFC) / (_dcMax - _dcMin + 1);
_theta = constrain(_theta, 0, _unitsFC - 1);

We can compute the angle position as shown above, as the datasheet says. Basically, it is converting the duty cycle reading to 0-360 degrees. Minimum and maximum duty cycle depends on servos. However, it is not good to put this function inside the interrupt because it is computationally heavy. It's better to only have simple integer operations like addition, subtraction, and comparison functions in interrupts.

Rotation Counter

Since the encoder only measures from 0 to 360, we need a rotation counter that can measure more than one rotation. The suggested function from the datasheet is like below.

int _dcMin = 30;
int _dcMax = 1071;
int _unitsFC = 360;
int _q2min = 90;
int _q3max = 270;
int _turns = 0;
double _theta;
double _thetaP;

float getAngle(){
  _theta = (_unitsFC - 1) - ((_dc - _dcMin) * _unitsFC) / (_dcMax - _dcMin + 1);
  _theta = constrain(_theta, 0, _unitsFC - 1);

  if((_theta < _q2min) && (_thetaP > _q3max)) _turns++;
  else if((_thetaP < _q2min) && (_theta > _q3max)) _turns --;

  if(_turns >= 0) _angle = (_turns * _unitsFC) + _theta;
  else _angle = ((_turns + 1) * _unitsFC) - (_unitsFC - _theta);

  _thetaP = _theta;
  return _angle;
}

This code changes rotation when the angle goes from quadrant 1 to quadrant 4 and vice versa. And use turns for calculating the absolute angle value. However, this does not work when the motor is moving fast. Therefore, when we do PID control for the motor, we use past output direction and angle change to fix the rotation counting. The below is PID loop with rotation counter fixing.

void gotoAngle(double targetAngle) {
  _targetAngle = targetAngle;

  // rotation counter fixing
  _angle = getAngle();
  if ((_output > 0) && (_angle - _angleP < -90)) _turns ++;
  else if ((_output < 0) && (_angle - _angleP > 90)) _turns --;

  // speed measure
  _angle = getAngle();
  _time = millis();
  _speed = 1000 * (_angle - _angleP) / (_time-_timeP); // deg/s
  _timeP = _time;
  _angleP = _angle;

  // offset is needed because of servo's deadband
  float offset = 8 * ((_targetAngle - _angle>0)-(_targetAngle - _angle<0));
  posControl.compute(); // motor PID control
  servo.write(constrain(_output + 90 + offset, 0, 180));
}

If output was positive but the angle suddenly goes negative by more than 90 degrees, that means that we need to increase the turns, and vice versa.

Full code

#ifndef esp32_parallex_h
#define esp32_parallex_h

#include "Arduino.h"
#include "PID.h"
#include "servo_esp.h"

class parallexServo
{
  public:
    parallexServo(int outPin, int feedPin, int channel)
    {
      pinMode(feedPin, INPUT);
      servo.setChannel(channel);
      servo.setPeriodHertz(50);
      servo.attach(outPin, 1200, 1800);
      _feedPin = feedPin;
      _outPin = outPin;

      _angle = getAngle();
      _targetAngle = _angle;
      _timeP = -1;

      posControl.set(&_angle, &_output, &_targetAngle, 0.4, 0.01, 0.02);
      posControl.setLimits(-90, 90);
      posControl.init();
    }

    float getAngle(){
      _theta = (_unitsFC - 1) - ((_dc - _dcMin) * _unitsFC) / (_dcMax - _dcMin + 1);
      _theta = constrain(_theta, 0, _unitsFC - 1);

      if((_theta < _q2min) && (_thetaP > _q3max)) _turns++;
      else if((_thetaP < _q2min) && (_theta > _q3max)) _turns --;

      if(_turns >= 0) _angle = (_turns * _unitsFC) + _theta;
      else _angle = ((_turns + 1) * _unitsFC) - (_unitsFC - _theta);

      _thetaP = _theta;
      return _angle;
    }

    void signalInterrupt() {
      if (digitalRead(_feedPin) == HIGH) {
        _pulseInTimeBegin = micros();
      }
      else {
        _pulseInTimeEnd = micros();
        unsigned long temp = _pulseInTimeEnd - _pulseInTimeBegin;
        if ((temp > 0) && (temp < 1500)){_dc = temp;}
      }
    }

    void gotoAngle(double targetAngle) {
      _targetAngle = targetAngle;
      _angle = getAngle();
      if ((_output>90) && (_angle-_angleP<-90)) _turns ++;
      else if ((_output<90) && (_angle-_angleP>90)) _turns --;
      _angle = getAngle();
      _time = millis();
      _speed = 1000*(_angle - _angleP)/(_time-_timeP); // deg/s
      _timeP = _time;
      _angleP = _angle;

      float offset = 8 * ((_targetAngle - _angle>0)-(_targetAngle - _angle<0));
      posControl.compute();
      servo.write(constrain(_output+90+offset, 0, 180));
    }

    void reset(){
      _angle = getAngle();
      _targetAngle = _angle;
      _targetSpeed = _speed;
    }

    float _speed;
    float _targetAngle;
    Servo servo;

  private:
    PID posControl;
    float _output;
    int _time;
    int _timeP;

    volatile int _feedPin;
    volatile int _outPin;
    unsigned long _dc;
    int _dcMin = 30;
    int _dcMax = 1071;
    int _unitsFC = 360;
    int _q2min = 90;
    int _q3max = 270;
    int _turns = 0;
    float _theta;
    float _thetaP;

    float _angle;
    float _angleP;

    volatile unsigned long _pulseInTimeBegin = micros();
    volatile unsigned long _pulseInTimeEnd = micros();
    volatile bool _newPulseDurationAvailable = false;
};

#endif

Example

#include "esp32_parallex.h"

#define motorPin 7
#define feedPin 5
#define motorCh 0

parallexServo motor(motorPin, feedPin, motorCh);
void IRAM_ATTR MotorInterrupt() {motor.signalInterrupt();}

// deg from the current state
void goTo(double targetDist){
  // get the current motor angles
  double cur = motor.getAngle();  
  double tar = motor._targetAngle + cur;

  int timeout = 3000;
  int start = millis();

  while((millis()-start < timeout) && (abs(tar-cur) > 2)){
    motor.gotoAngle(tar);
    cur = motor.getAngle();
  }
  // end the control
  motor.servo.write(90);
}

void setup(void)
{
  Serial.begin(115200);
  // set motor encoder interrupts
  attachInterrupt(digitalPinToInterrupt(feedPin), MotorInterrupt, CHANGE);
}

void loop(void)
{
  goTo(360 * 5);
  delay(5000);
  goTo(-360 * 5);
  delay(5000);
}