Autonomous PID movement

Technical Programming
1

Hi,

For autonomous mode I’m able to find the target and calculate a setpoint to turn to, but I’m not sure I’m actually using PID control correctly. Here is what I’m doing in pseudo code

class PIDCameraSource : public PIDSource
{
public:
	PIDCameraSource() : source_( 0.0f ) {
	}
	
	virtual Double PIDGet() {
		return source_;
	}
	
	void SetSource( Double source ) {
		source_ = source;
	}
	
private:
	Double source_;
};

class PIDCameraOutput : public PIDOutput
{
public:
	PIDCameraOutput() : output_( 0.0 ) {
	}
	
	virtual void PIDWrite( Float output ) {
		output_ = output;
	}
	
	Float GetOutput() const {
		return output_;
	}
	
private:
	Float output_;
};


static int currentDriveX_ = 0; // really a member variable, not a static

if( TargetFound ) {
    pidSource_.SetSource( currentDriveX_ ); // pidSource_ is a member variables
    pidController_.SetSetpoint( normalize_x_center_of_mass ); // as well pidController_
}

robotDrive_.Drive( speed, (currentDriveX_ = pidOutput_.GetOutput()) ); //  speed calculate elsewhere

Basically, my source is just my last PID output. Is that wrong? Obviously there is not direct 1-to-1 relationship between the a turn curve and the values actually sent to the two drive motors for tank control, but should I still try to calculate what the turn curve is from the two motors or is what I have sufficient?

Also, what’s the best to go about getting the PID constants right? I seem to be having a hard time calculating something that works and I’m not sure if it’s because I’m just using PID control wrong, or if my constants are just to far off.

2

You’re almost there.

Some tips:

  1. You don’t need to declare PIDGet() and PIDWrite() as virtual. The PIDSource::PIDGet() and PIDOutput::PIDWrite() functions are virtual because they are meant to be overwritten by you (which you did). Yours will not be inherited (presumably).

  2. You probably want to disable “pidController_” when you can’t see the target. Otherwise it is processing away in the background. If your controller has a non-zero integral gain (Ki), the output will wind up because the error is not being reduced as far as it knows. When it comes time to use the controller output again, the value might be garbage.

  3. You don’t generally need to explicitly compute the relationship between the camera input and motor output. This is what your control loop gains will do for you - it is far easier (and oftentimes more accurate) to empirically adjust the gains of the PID controller rather than compute the relationship mathematically. So instead of normalizing the x offset, just putting in the raw offset in pixels from the center of the frame is probably easier.

  4. This is the general PID implementing/tuning procedure I use:

A. MAKE SURE ALL OF THE SIGNS ARE RIGHT! Verify that the machine at least turns in the right direction to reduce the rror. Between the camera coordinates, handedness of your motor, and the polarity of the motor wiring, there are plenty of places where your control loop might accidentally be backwards. Sometimes it is harder to discern this than one would think (particularly with an untuned system). In step B I present an estimated Kp parameter that you should be able to use.

B. When tuning the system, start with 0 as Ki and Kd, and a small value for Kp. How small? Well, if your input is in pixels of offset, and the output is a bounded motor command in the range -1, 1], then how about a value of Kp = 0.025. That way your motor will get full power if the error is at least 40 pixels (40*0.025 = 1), and the power will decrease linearly if the error is less than that. This guess might be off by a power of 10 or more, but it should at least let you do the test in step A.

C. First tune Kp - get it so it moves “reasonably close” to the right spot relatively quickly, maybe with just a bit of overshoot. Then increase Ki (start with a very low value!). This will eliminate any residual error - i.e., it will move your bot from “reasonably close” to “dead on”. But too much Ki can really make your machine oscillate. Kd is seldom needed in many heavily damped systems (in other words, systems with a lot of friction, like many robot mechanisms), so only fiddle with that if you aren’t satisfied with the performance from the PI controller alone.

D. Don’t get caught up in trying to over-optimize your controller. It will only ever be so fast at reacting to changes and meeting your setpoint - this is a product of the physical dynamics of the system being controlled. Recognize when you are at these limits and let the mechanical team know if they’ve made a system with characteristics (underpowered motors, lots of inertia, too much friction, backlash in the gears/chain) that make fast control impossible.

Good luck!

3

Thanks soo much for your response, it will help tremendously!

Just to comment on my use of virtual, I like to leave the virtual there when overriding another virtual purely for documentation purposes (since sometimes it becomes confusing when reading old/foreign source code whether an argument is overriding a pure virtual, a virtual, or making it own’s, and it gives a hint to why the method is needed since it redirects you to one of the classes superclasses.

Thanks again!