Robot does not move in a straight path even after applying pid

I am trying to control a robot which has 3 dc motors fitted to it's base (Triangular base). I am controlling it through a ps3 controller.

The problem with the robot is that it does not move properly when given the command through ps3. I have tried to apply the pid algorithm to all the motors but i am having trouble in code as it does not work properly. The motor are not moving according to the command given through ps3. Also note that i have tuned the parameters of the motors individually so the problem is in the code itself and not in the constants of pid.

the motors are equipped with optical encoders and i am using arduino mega for my project.

can someone please correct my code as am having so much trouble in correcting it.

i have attached 2 files (i have called one function in another so as to make the code more readable).

Motor.ino (8.83 KB)

Motor_PID_Enc.ino (2.96 KB)

The PID process uses feedback to control something. An example is a thermostat. It measure temperature in the room, and uses that to turn the furnace off or on, depending on the relationship between the setpoint (what temperature you want the room air to be) and the measured value. The tuning parameters control overshoot, how long the furnace should run, etc.

What is your input? What is your setpoint?

Typically, PID would be used to make the robot travel in a straight line by collecting data from sensors that measure the deviation from that line. That would be the input. The setpoint is the distance you want to be from the line - 0, obviously.

It is not clear from your description what you are measuring or what your setpoint is.

Could this be the main problem? Should be count_T in rencoder3() I assume. (also the comment looks incorrect)

void rencoder2()  {                                       // pulse and direction, direct port reading to save cycles
  if (PIND & 0b00000001)    count_L++;                     // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #0)
  else                      count_L--;        
}

void rencoder3()  {                                       // pulse and direction, direct port reading to save cycles
  if (PIND & 0b00000100)    count_L++;                     // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
  else                      count_L--;        
}

Also these should be volatile:

long count_R = 0;
long count_L = 0;
long count_T = 0;

And you should turn off interrupts when accessing them (because otherwise they could get updated half way through reading them):

  long count_R_temp = count_R - lastCount_R;
  long count_L_temp = count_L - lastCount_L;
  long count_T_temp = count_T - lastCount_T;

I have corrected the mistakes that you pointed. Thank you so much for that. But motors still are not moving the way i want them to be.

I am taking the input from optical encoders that are attached to the dc motors. My setpoint is actually variable as i have to move the robot in an arena ,so i have to change direction of robot manually by ps3 controller.

I am applying PID for the first time so please guide me in detail what should i do in order to make the robot move smoothly and in straight path when i want it to be.

All your help is greatly appreciated. Thank You.

My setpoint is actually variable as i have to move the robot in an arena ,so i have to change direction of robot manually by ps3 controller.

The PS3 controller is sending something to the Arduino on the robot. Feel free to share what that is, and how that defines a straight path for the robot to follow.

I am taking the input from optical encoders that are attached to the dc motors.

Are you assuming that the wheels can't possibly slip? Are you assuming that the wheels are exactly the same diameter? I suspect that the answer to both of these is yes. The reality, though, is that neither one is a valid assumption.

It looks to me like you are setting direction based on the specific controller input.

    else if(PS3.getButtonClick(RIGHT))
    {
        Serial.print("RIGHT");
        digitalWrite(InA_R, LOW);                       
        digitalWrite(InB_R, HIGH);
        digitalWrite(InA_L, LOW);                       
        digitalWrite(InB_L, HIGH);
        digitalWrite(InB_T, LOW);                       
        digitalWrite(InA_T, HIGH);

So should the PID be controlling speed only? Or both speed and direction? Or an adjustment to some base speed as your variable names perhaps suggest?

        offset_R = constrain(offset_R, -255, 255);
        offset_L = constrain(offset_L, -255, 255);
        offset_T = constrain(offset_T, -255, 255);
        analogWrite(PWM_R,offset_R); 
        analogWrite(PWM_L,offset_L);
        analogWrite(PWM_T,offset_T);

Note that analogWrite() is expecting a value of 0 to 255. If -255 is full reverse you'll have to handle the direction part separately first.

If the 'offset_X' variables are meant to modify a base speed, as is suggested by the fact that your PID routine appears to take the variable 'torque' as the set point, then that's missing i.e. you're just missing that base speed when you call analogWrite().

The above is all a bit of a guess as it's not totally clear how it's supposed to work. But if you try and do this:

PaulS:
The PS3 controller is sending something to the Arduino on the robot. Feel free to share what that is, and how that defines a straight path for the robot to follow.

and map/describe the process from input to output it will make it clearer for you (as well as everyone else) how everything is working or supposed to work.

The PS3 controller is sending something to the Arduino on the robot. Feel free to share what that is, and how that defines a straight path for the robot to follow.

So, my objective is that when i press the "UP" button on my ps3, the robot should move forward in a straight path till the button is pressed. Similarly when all the other commands like "RIGHT", "LEFT", "DOWN" are given, the robot should move in that direction in a straight path.

if(PS3.getButtonClick(UP))
{
Serial.print("UP");
digitalWrite(InA_R, LOW);
digitalWrite(InB_R, HIGH);
digitalWrite(InA_L, LOW); //since the base has 3 motors having omni wheels, i have to control
digitalWrite(InB_L, HIGH); //only two wheels to move it in forward direction.
digitalWrite(InB_T, LOW);
digitalWrite(InA_T, LOW);

}

am trying to calculate speed from the readings from optical encoder and the apply pid on speed on all three motors so that every motor rotates at same speed and hence robot should move in straight path. (that;s how it works right? :sweat_smile: am sorry am still a noob)

Since i am moving the robot in an arena, i want it to move accurately and smoothly and according to the commands that i give to it. This is the reason why i want it to travel in straight path so that i don't have to correct the position of robot every now and then.

It looks to me like you are setting direction based on the specific controller input.

That's exactly what am trying to do.

So should the PID be controlling speed only? Or both speed and direction? Or an adjustment to some base speed as your variable names perhaps suggest?

The PID should control the speed only. The direction of the robot is changed only when i give the commands through ps3. After the robot has changed the direction, it should again move in straight path till i give him the command to change it's direction again.

shyam0099:
am trying to calculate speed from the readings from optical encoder and the apply pid on speed on all three motors so that every motor rotates at same speed and hence robot should move in straight path.

I think you need to explain (or better, show) the physical configuration of these 3 wheels. Is it something like this?
I would also show what directions up/down/left/right mean in relation to this robot (i.e. put some arrows on your picture). Perhaps this sounds like a stupid question but at least for me it is hard to visualise what you're working with without any information from you.

shyam0099:
The PID should control the speed only.

What do you expect/want the output of the PID to be? A number that you can use directly in analogWrite()? Or an offset/adjustment to some standard base PWM value?

Regardless, the input to analogWrite() must be between 0 and 255 but you are trying to give it a number between -255 and 255.

physical configuration of wheels is something like this :

What do you expect/want the output of the PID to be? A number that you can use directly in analogWrite()? Or an offset/adjustment to some standard base PWM value?

I want to use the output of pid directly in analogWrite(). Is this a better choice than an adjustment to base pwm value?

Regardless, the input to analogWrite() must be between 0 and 255 but you are trying to give it a number between -255 and 255.

I will correct this mistake in the code. Thanks for pointing it out.

For up and down movements, you shouldn't be moving the top wheel, right?

Can you explain what isn't working still? I.e. exactly what you expect to observe and what you actually observe.
Is it just particular movements? Or all movements?

It might be worth completely removing the PID to check if the rest of the code is doing roughly the right thing. Then when you are sure that it is, add the PID back to get more controlled movement.

Can you explain what isn't working still? I.e. exactly what you expect to observe and what you actually observe.
Is it just particular movements? Or all movements?

The problem is that the motors are not moving at the same speed no matter what commands i give to it. This means that my PID code is not coming into action. This is the main problem that i have because i can't figure out why output of pid is not getting applied on to the motors.

It might be worth completely removing the PID to check if the rest of the code is doing roughly the right thing. Then when you are sure that it is, add the PID back to get more controlled movement.

I did this and found that the motors work in same way with or without the pid algorithm. This means there is some error in code because of which the algorithm is not coming into action.

I request you to please modify my code so that my robot can move in a way that i want it to be.

Again Thank you for all your help. It really means a lot.

shyam0099:
The PID should control the speed only. The direction of the robot is changed only when i give the commands through ps3. After the robot has changed the direction, it should again move in straight path till i give him the command to change it's direction again.

But you have 3 independent variables with your holonomic robot, so a single control loop is clearly not enough.
In fact 3 loops are needed, together with various conversions. You'll need good 2D geometry. Here's a starting
place: mobile robot - How to program a three wheel Omni? - Robotics Stack Exchange

In fact 3 loops are needed, together with various conversions

Can you please tell me how to code this? My head hurts even thinking of this as i have been stuck with 1 loop since last week.

shyam0099:
The problem is that the motors are not moving at the same speed no matter what commands i give to it. This means that my PID code is not coming into action. This is the main problem that i have because i can't figure out why output of pid is not getting applied on to the motors.

Have you looked at the output values the PID is producing?

I would take a single motor and print some stats to the serial monitor while it runs:

  1. Target speed (setpoint)
  2. actual speed as measured by the encoders
  3. output from PID

Also, you have this variable STD_LOOP_TIME in the speed calculation but I can't see anywhere in the code that you actually enforce a regular loop time.

Given that you have made changes to your code, it would also be worth reposting it.