Go Down

Topic: Assistance requested to implement a steering PID on a differential drive rover (Read 226 times) previous topic - next topic

mdbirley

Hi All
I am working on an autonomous rover. It is driven by 2 independent 12v motors that are controlled by an Arduino Mega through a Sabertooth 2 * 12 motor controller in mixed mode. To get feedback on direction I am using an HMC 5883 compass. To get a position I am using a pair of ublox M8P receivers in rtk mode.

The arduino sketch amongst other things calculates the remaining distance and course to the next waypoint. Also the error between the actual and target heading "errorHeading"

The sabertooth is set up in mixed mode.. If I "write" a command to the attached servo the command will change the speed or direction of the rover motors. For example leftRightMotor.write(turn); Turn being a number from 0 to 180. with 90 and stopped o full reverse and 180 full forward
The way the rover is currently controlled is based on the value of errorHeading. If the errorHeading is positive the write command associates a number from 0 to 90 and if the error is negative a number between 90 and 180 is sent. . If the heading error is between -5 and +5 degrees of the target direction the rover drives straight.
Currently this relationship is hard wired. Although this is working as my test area is on a slight slope the outcome is jerky.

I would like to implement a PID to improve  the steering. From my reading I should use headingError as the input to the PID. The set point would be 0 degrees of headingError ie driving straight. The output would be some value to be sent in the write statement to cause the motor to bring the rover to a 0 headingError.

I tried using the arduino PID library with Kp=1, Ki=0,Kd=0. however I was not sure if I fully understood how to fully implement this as there are so many variables I could not tell if it was working

If someone can confirm how I would actually implement in Arduino code a steering PID I would be grateful.

Lastly to counteract the slope would I also need a odometer PID for the wheels for will this be dealt with in the steering PID

regards
Max

jremington

The steering system you have chosen is not optimal for PID steering.

It is much easier to just control the speed (power) to the motors individually and differentially.

Example: the speed of MotorL is set to (base speed - correction) and MotorR is set to (base speed + correction), where correction = Kp*(heading error). Later add Kd and Ki if needed.

If the heading error is positive, correction is positive, MotorL slows down and MotorR speeds up, and the model veers to reduce the heading error. Vice versa for heading error negative.

Pay close attention to compass wrap when calculating heading error. Here is one way to do it:

Code: [Select]
// routine to calculate heading error in degrees, taking into account compass wrap

int heading_error(int bearing, int current_heading)
{
 int error = current_heading - bearing;
 if (error >  180) error -= 360;
 if (error < -180) error += 360;
 return error;
}

mdbirley


Thank you for taking the time to answer. Your efforts are really appreciated.

To control the rover I actually write 2 values to the motor controller. As the motor controller is setup on MIXED MODE the first command sets the forward and reverse speeds of the 2 motors. So a write value of 0 is 100% reversed, 90 is stop and 180 is 100% forward. I generally drive forward at 120.

The second command controls the speeds of the 2 wheels to create a turn.
0 stops one wheel and turns the other at 100%, 90 is straight ahead with both wheels and 180 turns the other way. I tend not to reverse the motors as I do not need to pivot.
As the differential steering is very very positive and the wheel base is short on this rover I tend to keep the turning commands to between 60 and 120 otherwise it turns too fast and unsettles the compass.
Example enclosed

Code: [Select]

   if (turnDirection == straight)
               {
               forwardSpeed = 115;
               forwardBackMotor.write(forwardSpeed);
               turn = 90;;
               leftRightMotor.write(turn);
               }
               

    if (turnDirection == left)
               {
                forwardSpeed = 115;   
               forwardBackMotor.write(forwardSpeed);
               turn = 130;
               leftRightMotor.write(turn);
               } 
     
    if (turnDirection == gentleLeft)
               {
                forwardSpeed = 115;   
               forwardBackMotor.write(forwardSpeed);
               turn = 100;
               leftRightMotor.write(turn);
               } 
                   
     if (turnDirection == gentleRight )
                {
                forwardSpeed = 115;   
                forwardBackMotor.write(forwardSpeed);
                turn = 80;
                leftRightMotor.write(turn);
                }   

      if (turnDirection == right )
                {
                forwardSpeed = 115;   
                forwardBackMotor.write(forwardSpeed);
                turn = 50;
                leftRightMotor.write(turn);
                }





I have also programmed relationships between the heading error and the turn commands.   


Code: [Select]

    // calculate which way to turn to intercept the targetHeading
    if (abs(headingError) <= HEADING_TOLERANCE)      // if within tolerance, don't turn
      turnDirection = straight;
       
      else if (headingError < -30)
      turnDirection = left;

     else if ((headingError < -5) and (headingError >= -30))
      turnDirection = gentleLeft;
     
    else if (headingError > 30)
      turnDirection = right;

     else if ((headingError > 5) and (headingError <= 30))
      turnDirection = gentleRight;
   
 //   else
 //     turnDirection = straight;



This is all working well but it is a little basic and jerky.

I have never implemented a PID so I currently can not quite see all the steps required to get it working. 

I have copied recently the PID basic example into my code but I am not sure if it is working.

I would appreciate if you can show me the actual lines of Arduino code that are required. Once I get it once I will
be able to do this on my own in future. Its just the first time that is the difficulty.

Also if there is a better way to drive a straight line I would appreciate the guidance. What ever the "best" way I still want to implement a PID so I can really understand what is going on

Many thanks

Max





jremington

Quote
I would appreciate if you can show me the actual lines of Arduino code that are required.
I will be happy to post the code that I use, based on what I've already posted.

However, I am not interested in trying to adapt it to your approach to steering, because I don't think your approach will work with PID. A jerky or nonlinear response can be fatal for PID controllers.

jremington

In rereading this second explanation, perhaps PID will work with that controller after all.

Quote
The second command controls the speeds of the 2 wheels to create a turn.
0 stops one wheel and turns the other at 100%, 90 is straight ahead with both wheels and 180 turns the other way. I tend not to reverse the motors as I do not need to pivot.
However, "turning behavior" must be proportional to the difference from 90, i.e. 92 should result in a very large turning radius.

Then, just offset the PID output as follows. Note: I don't understand your examples of the motor control commands.

Code: [Select]


// in loop() function ...

 mag_heading = wrap360(v[0] + yaw_offset); //yaw = v[0] and correct for magnetic North

// heading error and PID steering, "point_bearing" is the setpoint

 error = heading_error(point_bearing, mag_heading);  //wrap to +/- 180 degrees
 
 //error is positive if current_heading > bearing (compass direction)
 //if  (turn > 90) acts to reduce left motor speed, bear left

 int turn = 90 + (kp*error);  //may need to subtract kp*error and/or include Ki, Kd

 set_output(turn); //command the turn
 



Go Up