Does anyone have a good reference to a Drive Straight PD loop for encoders? I am new to PID loops in general, so just looking for a basic PD loop that gets the job done. Thanks in advance!
I think this worked. This was for encoder backs, so you will need to change nMoterEncoder] to SensorValue() for OSE’s.
// function to run drive motors for specified distance in units defined by WHEEL_DIAM then stop the drive motors
// +dist = forward, -dist = reverse.
void driveDistance(int dist)
{
nMotorEncoder[drive_LF] = 0; // reset the ime values to 0
nMotorEncoder[drive_RF] = 0;
driveLastError = 0; // update driveLastError
driveTarget = fabs(dist/(WHEEL_DIAM*PI) * ENCODER_TICKS); // convert distance into encoder ticks. WHEEL_DIAM and ENCODER_TICKS set in globals
driveError = driveTarget - fabs(nMotorEncoder[drive_LF]); // initial error calculation
while (fabs(driveError) > DRIVE_ERROR_THRESH) // while more than DRIVE_ERROR_THRESH from target
{
driveError = driveTarget - fabs(nMotorEncoder[drive_LF]); // update error value
int drivePower = sgn(dist) * (drivePD_Kp * driveError) + (drivePD_Kd * (driveError - driveLastError)); // initial PD power calculation
drivePower = fabs(drivePower) > DRIVE_MAX_PWR ? sgn(drivePower) * DRIVE_MAX_PWR : drivePower; // limit power if calc is over max
// drive straight power modifier
int rDiff = fabs(nMotorEncoder[drive_LF]) - fabs(nMotorEncoder[drive_RF]); // check for right encoder difference
int rMod = sgn(rDiff)*drivePower*.1; // rMod = 10% drivePower in the direction of rDiff
set_drivePower( drivePower , drivePower + rMod );
driveLastError = driveError; // update driveLastError
wait1Msec(25);
}
set_drivePower( 0 , 0 ); // stop motors after target is reached
}
5 Likes
For driving straight the most important thing is to have one PID loop for driving robot forward and another separate control loop for small heading corrections to keep the robot straight, which is exactly what @Doug Moyers code does.
That code could work great if you have perfectly balanced drivetrain and matched motors. However, that is very hard to achieve in the real life. One side will inevitably have more friction than the other. To handle that you will need to cap drivePower coming out of the forward PID loop about 10-15% below the max power that motors could accept.
The previous post doesn’t specify the value for DRIVE_MAX_PWR, which we would set to 85 in order to keep 393 motors in the linear region between 80 and 90 power units. To keep it symmetric we would also calculate the current position as an average between left and right encoders, and set the final power as:
set_drivePower( drivePower - rMod/2 , drivePower + rMod/2 );
Another improvement could be to maintain integral for the rDiff error and use it in calculation of rMod. Under normal conditions nonzero values for the accumulated integral would tell you if one side of the drivetrain had more friction losses than the other. Also, if robot’s load is unbalanced it could improve straightness and accuracy of the drive path if you carry capped value of the integral between the drive steps.
2 Likes
There is one more important thing I forgot to mention. With Cortex 393 motors we would use Smart Motor Library that would protect motors from overheating both in driver and autonomous mode. It would limit motor’s max power and the slew rate.
If you are not using any library that controls slew rate you have to do it yourself at the output stage of the move forward PD loop. You need to remember base motor power at the last step and let new power only increase no more than, let say, 5 power units per 25 msec time slice. In addition to avoid shocking motor’s gears, this will ensure that robot accelerates gradually and wheels do not slip. If they do - you lose your accurate travel count.
Each robot has a unique drivetrain power to mass ratio and you need to experiment what slew rate is the best to get the robot accelerate as fast as possible, without slipping wheels and losing accuracy in its position estimate.
1 Like
good points @technik3k. I would recommend a much lower DRIVE_MAX_POWER – slower motion is generally more accurate. Consider starting at 45, tuning your PD constants until you like the results, then dialing up the speed until you find the fastest speed you can run with an accuracy that you can accept (or lets you finish all actions in the time limit.)
On a related note, my teams have found (to their surprise) that very slow speeds are better than high speeds for climbing the platform. They are using V5 though, so I would not know if this carries over to 393.
1 Like