Implement a PID control in C++ for a motor with absolute encoder

Question

Sorry if my question is too stupid, but I can't figure out how to solve my problem. I have a motor with a gearbox and I also have an absolute encoder mounted on the gearbox shaft. I need to make the output shaft rotate in a range from -90 to +90 and it is centered in 0°. Now, when the shaft is in 0°, then the encoder outputs 1010, when it is at -90°, the encoder outputs 1120 and when it is in +90° it outputs 900.

When the shaft is in 0° and has to reach +90°, the motor must rotate clockwise and when it needs to reach -90°, it needs to rotate counterclockwise.

I would like to command the motor by only giving it the position in degree. For example, I'd like to have a function like:

move_motor(1, 45°)

int move_motor(id_motor, pos){
 read current motor position
 // motor is at 0°
 make motor #1 spins clockwise until encoder returns 955
}

I think that a PID controller would be a smart solution, but I really do not know how to implement it in C++, sorry, I'm not a developer. Or do you suggest just to use if/else statements?

EDIT:

In order to make the motor move, I use this function:

void move_motor(motor_id, direction)

and it makes the motor spin in counterclockwise or clockwise depending on the second parameter To stop the motors:

void stop_motor(motor_id, 0)

and this other function:

int get_enc(encoder1)

returns an integer depending on the encoder1 readings.

So for example, to reach the desired position it should be:

while (get_enc != desired_position){
move_motor(motor_id, direction)
}

but the direction should be handled, too.

Really more of an engineering question than coding. You probably should check out https://electronics.stackexchange.com/ and review FIR and IIR discrete filters for implementing PID. — doug, Sep 21 '22 at 17:51
It seems to be a wierd log scale. From -90 => 1120, 0 => 1010, +90 => 900. Since it's not linaer, do you have the formula for it or is that formula you need help with? Btw, the picture says +90 => 300, not 900. — Ted Lyngmo, Sep 21 '22 at 17:57
It is proportional, for example, +45° => 955 or -45° => 1065. It's 900, it is a "9" even if it is not well defined. — Marcus Barnet, Sep 21 '22 at 18:00
I would like to understand if I should use a formula or a pid controller to set the position in degrees and to reach it accurately. — Marcus Barnet, Sep 21 '22 at 18:05
@MarcusBarnet Is there any sort of documentation that came with the motor? For example, how are you reading the encoder? And is there a way to command the motor to move clockwise/counterclockwise? — cwbusacker, Sep 21 '22 at 18:06
So, `unsigned angle2pid(float angle) { return 1010 - angle * 55 / 45; }` or, to limit it, `unsigned angle2pid(float angle) { return std::clamp(1010-angle*55/45, 900.f, 1120.f); }` — Ted Lyngmo, Sep 21 '22 at 18:07
I do not think it is correct since the angle is a user parameter. In order to retrieve the encoder readings given a position in degrees, should be: 1.22*(pos)+1010 since the encoder return 1.22 for each degree. The problem is that I do not know how to handle all the positioning, i.e., how to consider the current position and how to minimize the positioning — Marcus Barnet, Sep 21 '22 at 18:10
The formula I gave gives the correct output for all your examples if the angle is given in degrees. — Ted Lyngmo, Sep 21 '22 at 18:11
I think that what I need is to understand what's inside the angle2pid function. I guess I should use some kind of controller based on the encoder feedback — Marcus Barnet, Sep 21 '22 at 18:12
Do you have the angle in degrees and want a number between 900 and 1120 as output? — Ted Lyngmo, Sep 21 '22 at 18:13
@MarcusBarnet That function converts the encoder's value to degrees for you. So, to move to position ```+90``` you just have to use the function given above and send it degrees For example, calling ```angle2pid(90.0)``` will return ```900``` — cwbusacker, Sep 21 '22 at 18:14
@TedLyngmo: yes, I need a number as output between that range and then a function that make the motor spin in the correct direction, depending on its current position, until reach the correct encoder reading for the given direction. So, I should handle the current position and decide in which direction I have to make the motor spin to reach the desired position by minimizing the error. — Marcus Barnet, Sep 21 '22 at 18:17
@cwbusacker: thank you, yes I understand it, but I also need to implement the positioning of the motor and how to decide how the motor have to spin depending on the current position. — Marcus Barnet, Sep 21 '22 at 18:18
@MarcusBarnet Do you know the current angle of "the thing" in relation to some baseline? — Ted Lyngmo, Sep 21 '22 at 18:20
@MarcusBarnet The command to make the motor spin depends on the motor, hence why I ask for documentation of the motor's interface. — cwbusacker, Sep 21 '22 at 18:21
let's consider that move_motor(motor_id, direction) move the motor and get(encoder1) returns the integer related to the encoder reading — Marcus Barnet, Sep 21 '22 at 18:22

score 2 · Answer 1 · answered Sep 21 '22 at 18:17

2

Here's how I've understood it:

  input   output
----------------
<= -90°    1120
   -45°    1065
     0°    1010
   +45°     955
>= +90°     900

Then this function would do that:

#include <algorithm>

unsigned angle2pid(float angle_in_degrees) {
    return std::clamp(1010 - angle_in_degrees * 55 / 45, 900.f, 1120.f);
}

std::clamp is used to limit the output between 900 and 1120.

answered Sep 21 '22 at 18:17

Ted Lyngmo

93,841
5
60
108

1

I feel that this partially answers the OP question, but it's also asking how to command the motor to go to those positions. In which case, no documentation on how to control the motor was provided. – cwbusacker Sep 21 '22 at 18:18
This surely solve partially my problem thank you a lot! I already know how to move the motors, I need to call move_motor(motor_id, direction) and it moves at a fixed speed. I need to understand how to make it move until I reach the desired encoder reading and if I need to implement some kind of PID control to improve the positioning. – Marcus Barnet Sep 21 '22 at 18:20
1

@cwbusacker Yes, I agree. I only answered the part I understood. – Ted Lyngmo Sep 21 '22 at 18:21
1

@MarcusBarnet Ok, I don't "see" it. Perhaps some situation-examples added to the question would make it clearer. – Ted Lyngmo Sep 21 '22 at 18:22
1

I'm going to add them in the main question. – Marcus Barnet Sep 21 '22 at 18:22
@MarcusBarnet Do you know if the ```move_motor(motor_id, direction)``` a blocking? Also, once moving how do you stop it? – cwbusacker Sep 21 '22 at 18:23
I added the information to the main topic. @cwbusacker I use stop_motor function and it stops. It is not blocking, the code can do other stuff in the meanwhile. – Marcus Barnet Sep 21 '22 at 18:27
1

@MarcusBarnet Ok, I may have to look at the extra information after some sleep. Right now the unitless entities `direction` and the `int` in `int get_enc(encoder1)` doesn't tell me much. What do they describe? Is direction a verb? A number? If so, what do the numbers mean? – Ted Lyngmo Sep 21 '22 at 18:33
direction can be 1 or -1 so they are integers, the int returned by get_encoder is the encoder readings, so for example if the encoder is in +90, the returned integer is 900. – Marcus Barnet Sep 21 '22 at 18:41
1

@MarcusBarnet It seems cwbusacker figured it out. :-) – Ted Lyngmo Sep 21 '22 at 18:41
@TedLyngmo yes, but it is not finished yet! :) :) the problem is a little bit more complex since the motor spins and the encoder readings change fast. – Marcus Barnet Sep 21 '22 at 18:45

cwbusacker · Accepted Answer · 2022-11-07T01:45:39.767

2

Here you go:


unsigned angle2pid(double angle_in_degrees) 
{
    return 1010 - angle_in_degrees * 55 / 45;
}

void move_motor_to_angle(int motor_id, double angle)
{
   static const int CLOSE_ENOUGH = 10;
   int currentPosition = get_enc(motor_id);
   int desiredPosition = angle2pid(angle);
   
   // IF WE ARE CLOSE ENOUGH, DO NOTHING...
   if(std::abs(currentPosition - desiredPosition) <= CLOSE_ENOUGH)
   {
       return;
   }
   
   if(desiredPosition > currentPosition)
   {
       move_motor_in_dir(motor_id, CLOCKWISE);
       while(desiredPosition > currentPosition)
       {
           currentPosition = get_enc(motor_id);
       }
       stop_motor(motor_id);

   }
   else if(desiredPosition < currentPosition)
   {
       move_motor_in_dir(motor_id, COUNTER_CLOCKWISE);
       while(desiredPosition < currentPosition)
       {
           currentPosition = get_enc(motor_id);
       }
       stop_motor(motor_id, 0);
   }
}

Note that the motor_id might be a different type which you'll have to slightly adjust. And perhaps the get_enc requires a different argument, but this is the idea.

Credit goes to @TedLyngmo who provided the angle2pid function.

edited Nov 07 '22 at 01:45

answered Sep 21 '22 at 18:37

cwbusacker

507
2
12

1

Great that you deciphered it :-) – Ted Lyngmo Sep 21 '22 at 18:41
1

Did my best. Obviously it may need some adjusting once tested with the real hardware... – cwbusacker Sep 21 '22 at 18:42
Thank you! This is a good starting point. The only problem here is that the encoder readings changes quickly when the motor spins so the if statement could be never reached or the motor could oscillated around the desired position. For example, when the motor is at 90°, the encoder returns 900, 899, 901, 900, 901, 889 and so on. It is for this reason that I would like to use some kind of PID controller or another solution to fix it – Marcus Barnet Sep 21 '22 at 18:44
1

Change the if statement to check if the currentPosition is between 910 or 890 for example. – cwbusacker Sep 21 '22 at 18:45
1

@MarcusBarnet Make a simple lowpass filter. You could take the mean value of the X latest readings for example. If it still jitters, increase X until it doesn't. – Ted Lyngmo Sep 21 '22 at 18:45
@TedLyngmo won't the low pass filter slow the execution of the code and lose the real time? If I well understood, I should, for example, read the first 10 samples and then use the filter before move the motor. Is it correct? – Marcus Barnet Sep 21 '22 at 18:48
@cwbusacker shouldn't the move_motor be inside the while() loop together with get_end()? – Marcus Barnet Sep 21 '22 at 18:48
@MarcusBarnet see edits to handle readings that are "close enough". I placed ```move_motor()``` before the ```while``` because I assumed that ```move_motor()``` starts the motor moving and won't stop until ```stop_motor()``` is called. – cwbusacker Sep 21 '22 at 18:50
@cwbusacker OK, do not worry, I will adjust the code when I'll try it on the real robot, tomorrow and then I'll update you, thank you! – Marcus Barnet Sep 21 '22 at 18:52
@cwbusacker what could be the smart solution to filter the last 10 samples for example? is there a standard function to do that? – Marcus Barnet Sep 21 '22 at 18:53
@MarcusBarnet No, you add one value to a buffer of `X` samples, then take the mean value from the buffer so the reaction will be instant. You only need to keep the buffer in memory between the call to the filter. – Ted Lyngmo Sep 21 '22 at 18:54
so, for example, inside the while(), int pos[10], pos[i] = get_enc(motor_id);, if i > 9, current_position = array_average? – Marcus Barnet Sep 21 '22 at 18:58
2

The problem with averaging out the motor positions while looping is that it will probably go past the point you want it to. With the current encoder reading, it will stop as soon as one reading goes slightly passed the mark. – cwbusacker Sep 21 '22 at 19:03
Yes, this will surely happen if I put the average inside the while. I was thinking to make the motor stops one step before while(desiredPosition < currentPosition)-1 but this would not be repeatable. I mean, if I command multiple times the motor to reach a position, it is not sure that it will stop always in the same point. Isn't any option to use a filter or a pif control to tune the position somehow? – Marcus Barnet Sep 21 '22 at 19:15
1

@MarcusBarnet Averaging should probably be done on the wanted angles. [Example](https://godbolt.org/z/bbG758Gn1) – Ted Lyngmo Sep 21 '22 at 19:17
@cwbusacker "it will probably go past the point you want it to." Yep, overshoot is the problem with direct driving the stepper. You can step it slowly enough that it doesn't overshoot. A PID controller is designed to prevent that by feeding back a velocity estimate. It's actually not that common for stepper motors to be driven by a PID controller but they are essential for linear motors. Usually the step rate is slowed down when the motor is near the desired position. – doug Sep 21 '22 at 21:04
@doug According to the OP, I believe the motor has a constant velocity. Otherwise, I would've added something like that in there. – cwbusacker Sep 21 '22 at 21:22
@cwbusacker If that's the case then he shouldn't be asking for a PID controller which requires a velocity as well as position input. And PID is in the title. – doug Sep 21 '22 at 22:28
Why the function angle2pid() has 55/45? where this product come from? I cannot understand from where these numbers come from. Thank you! – Marcus Barnet Oct 26 '22 at 18:19
@MarcusBarnet, the encoder reported 55 units for every 45 degrees. So, we add in the 1010 to get you back to zero degrees and compute 55/45 to switch units from degreed to encoder. – cwbusacker Nov 07 '22 at 01:35

Implement a PID control in C++ for a motor with absolute encoder

2 Answers2