[Solved] How to deal with sensor wrap-around data using PID

lasersmurf · April 1, 2021, 10:42pm

Hi,

I am using an UNO R3 on IDE 1.8.13 on windows.

I am trying to get a DC motor (large gearing ratio so it turns pretty slowly, only a few rpms) to turn to a specific angle which is measured by a magnetic encoder, specifically the as5600. I am using PIDv1.h library to adjust the motor based on input from the as5600.

AS5600 datasheet: https://ams.com/documents/20143/36005/AS5600_DS000365_5-00.pdf/649ee61c-8f9a-20df-9e10-43173a3eb323

PID Library: Arduino Playground - HomePage

The issue that I am running into is that the sensor will "wrap-around" from 0-4095 and vice versa. When this happens the error calculated by the PID library will cause a change in direction forcing the motor to turn all the way around again to try and meet the setpoint.

For example: if the setpoint is close to either 0 or 4095, if the slightest bit of overshoot happens - passes 4095 and goes to 0, 1, or 2. Rather than simply reverse directions a few positions, it will continue in the same direction because the error changes signs.

Here is the code I am currently working with:

#include <PID_v1.h>
#include <A4990DualMotorDriverCarrier.h>
#include <Wire.h>
#include <AS5600.h>

const int BUFFER_SIZE = 8;
char opCode[BUFFER_SIZE];
int dest = 0;
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
double Kp = 1, Ki = 0, Kd = 0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

//Specific Class declarations
A4990DualMotorDriverCarrier motors(3, 11, 9, 10); //Motor Driver pins (need to be PWM capable)
AMS_5600 ams5600; //Magnetic Encoder

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  
  //Initialize and detect ams5600 magnetic encoder
  if (ams5600.detectMagnet() == 1 )
  {
    Serial.println(F("Pan Detected."));
    Serial.print(F("Pan - "));
    switch (int x = ams5600.getMagnetStrength())
    {
      case 0:
        Serial.println(F("No Magnet."));
        break;
      case 1:
        Serial.println(F("Magnet too weak."));
        break;
      case 2:
        Serial.println(F("Magnet just right."));
        break;
      case 3:
        Serial.println(F("Magnet too strong."));
        break;
    }
  }
  else
  {
    Serial.println(F("Can not detect pan magnet"));
  }
  motors.setM1Speed(0);
  Setpoint = 1000;

  //turn the PID on
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(10);
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  //Used to change the setpoint via serial for testing purposes
  if (Serial.available() > 0)
  {
    int incData = Serial.readBytesUntil('\n', opCode, BUFFER_SIZE);

    if (incData > 0)
    {
      for (int i = 0; i < incData; i++)
      {
        switch (i)
        {
          case 0:
            if (opCode[0] == 'S')
              break;
          case 1:
            dest = opCode[i] - '0';
            break;
          case 2:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
          case 3:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
          case 4:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
        }
      }
      Setpoint = constrain(dest, 0, 4095);
    }
  }
  Input = ams5600.getRawAngle();
  myPID.Compute();

  if (Output > 0)
    Output = map(Output, 0, 255, 20, 255);
  else if (Output < 0)
    Output = map(Output, -255, 0, -255, -20);
  
    motors.setM1Speed(-Output); //Output gets inverted to correct motor direction
  Serial.print(Input); Serial.print("    "); Serial.println(Output);
}

Any suggestions to deal with this?

blh64 · April 1, 2021, 10:56pm

I guess my first question would be why are you using PID?
With these constants

double Kp = 1, Ki = 0, Kd = 0;

You are not doing any sort of proportional control, it just relies on the position error.

It would be easier to just do calculate the move/direction directly and ditch the PID library.

lasersmurf · April 1, 2021, 11:18pm

I just figured PID would be the fastest route to a solution that would correct for overshoot and slow down on approach. I guess those two things really aren't complex to do.

It would still be useful to know how to deal with wrap around when using PID.

At the moment all I care about is the position but speed and timing will definitely be apart of this project in later stages.

Thanks.

jremington · April 1, 2021, 11:46pm

The usual way to avoid wrap around is to have a setpoint of zero. The error is the deviation from zero, which also eliminates the problem of "shortest path to goal".

This can be accomplished for PID steering on a 2WD vehicle or twin screw boat, following compass directions (0 to 359 degrees), as follows.

heading_error = (heading - bearing + 540)%360 - 180; //heading error is in range -180 to 179

output = Kp*heading_error + (other terms if needed);

right_power = base_power + output; //if heading error is positive, steer left
left_power = base_power - output;
set_right_speed(right_power);
set_left_speed(left_power);

For your encoder, change 360 to 4096, etc.

Note: "bearing" is the desired direction of travel, "heading" is the measured direction of travel.

Idahowalker · April 2, 2021, 12:46am

If the destination is saved. 4095.

Another variable keeping track of the number of clicks or ticks or ones and zeros counts to 4097 (or counts to -1,-2).

A sum of 4095 - 4097 = -2, which would give a correction amount and direction. Might work.

RayLivingston · April 2, 2021, 1:00am

You have two separate problems:

You need to add code to your encoder handler to allow the position passed to the PID to exceed the encoder count, so when the encoder wraps around from 4095 to 0, it returns a count of 4096 instead of 0. And the opposite when moving the other direction. So, for example, perhaps two, or more, revolutions of the encoder represents a single revolution of the actual hardware position.
When calculating error, you need to look at which direction is the shortest path to your target position, and calculating the error magnitude, and direction accordingly.

And, contrary to what was stated earlier, when the I and D coefficients of a PID are both set to 0, it is a PURELY proportional control. In a great many cases, a PI controller is perfectly adequate, leaving only the D term set to 0.

dlloyd · April 3, 2021, 10:02pm

I am trying to get a DC motor (large gearing ratio so it turns pretty slowly, only a few rpms) to turn to a specific angle which is measured by a magnetic encoder, specifically the as5600.

Can you position things so that the encoder is near mid-range for the specific angle (setpoint) you're shooting for?

Since the encoder doesn't work full range (limited near 0 and 360 degrees), perhaps you can improve things at the limits by using the CONF register to set new hysteresis and filtering options.

lasersmurf · April 6, 2021, 6:03pm

Thanks for the responses.

@jremington - It seems this type of solution would make the most sense. I know modulo math is probably the right way to go. I am not exactly sure how I would make the error, in my case, deviation from zero. It seems like that would require changing the "zero" at each move command?

@idahoWalker - I wrote up some code that doesn't use PID that implements a form of what your saying but it isn't as intelligent as PID when approaching the setpoint. Because of the load attached to the motor there isn't a certain point at which the voltage is too low for the motor to turn the load (a light fixture). Roughly around 20/255. Although dllloyd's suggestion about adjusting the Hysteresis might fix some issues with this.

@RayLivingston - I hadn't thought about doing any sensor data manipulation, at the moment I am using raw data from the sensor. The sensor is located on the output shaft after the gearbox, which makes one rotation of the raw sensor values the full range of 0-4095. Would reducing the overall 0-4095 range to correlate to 0-90deg then 4 "rotations" would be the equivalent of one actual rotation? Is that what you mean?

@dlloyd - I think this would solve the problem with my non-PID version. Adjusting the hysteresis after the move so that it wont toggle by 1/4095 positions would be really useful.

Here is my non-PID code.

#include <A4990DualMotorDriverCarrier.h>
#include <Wire.h>
#include <AS5600.h>
#define motorMin 10
#define motorMax 255
const int switchPoint = (2047 / 2) + 1;
const int BUFFER_SIZE = 8;
char opCode[BUFFER_SIZE];
int dest = 0;
int input, error, output, setpoint = 0;

//Specific Class declarations
A4990DualMotorDriverCarrier motors(3, 11, 9, 10); //Motor Driver pins (need to be PWM capable)
AMS_5600 ams5600; //Magnetic Encoder

void setup()
{
  motors.setM1Speed(127);
  Serial.begin(230400);
  Wire.begin();

  //Initialize and detect ams5600 magnetic encoder
  if (ams5600.detectMagnet() == 1 )
  {
    Serial.println(F("Pan Detected."));
    Serial.print(F("Pan - "));
    switch (int x = ams5600.getMagnetStrength())
    {
      case 0:
        Serial.println(F("No Magnet."));
        break;
      case 1:
        Serial.println(F("Magnet too weak."));
        break;
      case 2:
        Serial.println(F("Magnet just right."));
        break;
      case 3:
        Serial.println(F("Magnet too strong."));
        break;
    }
  }
  else
  {
    Serial.println(F("Can not detect pan magnet"));
  }
  setpoint = 1000;
}

void loop()
{
  //Used to change the setpoint via serial for testing purposes
  if (Serial.available() > 0)
  {
    int incData = Serial.readBytesUntil('\n', opCode, BUFFER_SIZE);

    if (incData > 0)
    {
      for (int i = 0; i < incData; i++)
      {
        switch (i)
        {
          case 0:
            if (opCode[0] == 'S')
              break;
          case 1:
            dest = opCode[i] - '0';
            break;
          case 2:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
          case 3:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
          case 4:
            dest = (dest * 10) + (opCode[i] - '0');
            break;
        }
      }
      setpoint = constrain(dest, 0, 2047);
    }
  }
  input = ams5600.getRawAngle();
  input = map(input, 0, 4095, 0, 2047);
  error = input - setpoint;
  Serial.print("Error - "); Serial.print(error);
  Serial.print("  Input - "); Serial.println(input);
  if (error == 0)
    motors.setM1Speed(0);
  if (error != 0)
  {
    if (error > 0)
    {
      if (error <= switchPoint)
      {
        if (error > 0 && error < 100)
        {
          error = constrain(error, motorMin, motorMax);
        }
        motors.setM1Speed(error);
      }
      else
      {
        if (error < 0 && error > -100)
        {
          error = constrain(error, -motorMax, -motorMin);
        }
        motors.setM1Speed(-error);//Passup through 4095 to 0
      }
    }
    else
    {
      if (-error <= switchPoint)
      {
        if (error < 0 && error > -100)
        {
          error = constrain(error, -motorMax, -motorMin);
        }
        motors.setM1Speed(error);
      }
      else
      {
        if (error > 0 && error < 100)
        {
          error = constrain(error, motorMin, motorMax);
        }
        motors.setM1Speed(-error);
      }
    }
  }
}

dlloyd · April 6, 2021, 7:50pm

You might find it helpful to saturate the data (as in PID) rather than constrain it.
I think saturate would work as in Figure 29: Output Characteristics with Reduced Output Range (10%-90%)
Constrain works by eliminating the deadband at each limit, but only in code ... its still there in the raw data.

Saturate:

if (error > motorMax) error = motorMax;
else if (error < motorMin) error = motorMin;

jremington · April 6, 2021, 8:42pm

I am not exactly sure how I would make the error, in my case, deviation from zero. It seems like that would require changing the "zero" at each move command?

The error is the difference between the current shaft angle and the target shaft angle.

The only thing you need to change is the target shaft angle, if that changes. The PID error calculation, with a setpoint of zero, looks like this in your case. It could hardly be simpler or more straightforward, and it automatically chooses the shortest path to the goal.

Angles are measured in encoder units, of course.

angle_error = (current_angle - target_angle + 6144))%4096 - 2048; //angle error is in range -2047 to 2047;
output = Kp*angle_error; // + (other K terms if needed)

lasersmurf · April 6, 2021, 9:41pm

@jremington - Thank you! That works very well. I was confusing the setpoint of the PID error (which in general is 0) with the target angle.

@dlloyd - Is there a difference between constrain and what you are proposing?

error = constrain(error, motorMin, motorMax);


if (error > motorMax) error = motorMax;
else if (error < motorMin) error = motorMin;

dlloyd · April 6, 2021, 9:56pm

@dlloyd - Is there a difference between constrain and what you are proposing?

Ha, none at all! ... was getting rushed and confused with map()

tomascalvario · May 21, 2021, 8:46pm

Hi,

I'm dealing with something like this. First I translated the sensor output from a digital count to a float in degrees. Now the readings go from -180 to 180. I also have a switch/button to 'reset' the zero position and offset the sensor output. Now the PID works great from -170 to 170 or so. As the problem is basically the same (going over the rollover from 4096 to 0 or from 180 to -180), I was thinking that maybe is just a matter of internally and temporarily 'offset' the sensor again just to let the math work without discontinuities.

jremington · May 21, 2021, 10:47pm

To avoid the problem of compass wrap in PID applications, most people use a setpoint of zero. Then the course error is just heading - bearing. Please reread post #4 above.

If heading and bearing are measured on the scale with 0-359, all wrap problems go away using the modulus operator (integers assumed):

course_error = (heading - bearing + 540)%360 - 180; //error is in range -180 to 179

system · September 18, 2021, 10:48pm

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