PID ... Iterative Robot Question (C++)

[duffany1]

Hi, so I am having a bunch of trouble. (C++)

We didn't really feel like using the PIDController on this year's bot. So, we decided to create a PID loop ourselves. And, it works extremely nicely. We have everything tuned properly, and we even got first place at the WPI Regional with this PID code. So really, nothing is wrong with the PID loop itself.

However, we have a problem. As I am trying just now to get autonomous working, I realized that the timing on the PID loop is not quite right.

We are using the IterativeRobot class in order to call the PID loop periodically (and according to the documentation, the period is 1/10000 of a second, shown here: wpi.edu > WPI Robotics Library: IterativeRobot Class Reference).

We have taken advantage of the fact that PID gets called periodically and we have just added a simple timer to that loop that we can set at any time and have it automatically decrement.

However, this timer appears to be roughly twice as slow as it should be! We printed it to the display and every two seconds the value of dly_tmr appeared to be decrementing by one.

Can anyone help?
At this point I am unsure as to whether I am even calling pid() in the correct manner. Should I even be using the IterativeRobot class for this?

Thanks in advance!!

Here's the code (massively wittled down, of course)

#include "WPILib.h"
#include <math.h>
#include "Utilities.h"
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>


// IRQ
const double IRQ_RATE = 10000.0; // period in microseconds
const float irq_rate_ms = IRQ_RATE / 1000; // period in milliseconds
const float irq_rate_sec = IRQ_RATE / 1000000; // period in milliseconds

class Team195Robot2011 : public IterativeRobot
{
       
public:
	
	Team195Robot2011(void):
		
		// Construction
		
	{
		GetWatchdog().SetExpiration(0.1);
	}
	
	void pid(void) //PID_IRQ
	{
		
		// Getting pot values, PID calculations, setting motor positions, etc.
		
		dly_tmr -= irq_rate_sec;

	}
	
	
	void RobotInit(void)
	{
		// Functions
	}

	// @AUTONOMOUS

	void AutonomousInit(void)
	{
		GetWatchdog().SetEnabled(false);
	}
	
	void AutonomousPeriodic(void)
	{
		pid();
	}

    void AutonomousContinuous(void)
	{
    	// Code
	}
	
	// @TELEOPERATED

	void TeleopInit(void)
	{
		GetWatchdog().SetEnabled(false);
	}
	
	void TeleopPeriodic(void)
	{
		pid();
	}

	// @MAIN 
	
	void TeleopContinuous(void)
	{
		
		// Super secret codez.
		// Print dly_tmr to the display.
	}

	// @DISABLED FUNCTIONS 
	
	void DisabledInit(void)
	{
	}
	
	void DisabledPeriodic(void)
	{
	}

	void DisabledContinuous(void)
	{
	}
	
};

START_ROBOT_CLASS(Team195Robot2011);

[Kingofl337]

You really should use the PIDController, it makes it's own task and everything.
All you need is...

AnalogChannel armtiltpot = new AnalogChannel(4);
Jaguar armtilt = new Jaguar(4);

//P I D input output update_rate
PIDController armtiltcontroller = new PIDController(5, 0, 0, armtiltpot, armtilt, .005);

armtiltcontroller.setEnabled();

Then call: armtiltcontroller.setSetpoint(2.5); to change the setpoint.


Based on what I'm seeing it looks like you code should work.

[Jared Russell]

The xxxPeriodic() functions are indeed called at 50Hz (nominally, to sync the function with the arrival of new data from the DS) rather than the 100Hz specified in the documentation. I suspect the documentation is simply out of date.

[boomergeek]

The default TeleopContinuous() shipped in 2011 WPILib code:

/**
* Continuous code for teleop mode should go here.
*
* Users should override this method for code which will be called repeatedly as frequently
* as possible while the robot is in teleop mode.
*/
void IterativeRobot::TeleopContinuous()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
taskDelay(1);
}

The taskDelay(1) allows your task to give up the processor for a very short period of time every loop instead of staying in a tight loop and using 100% of the processor until TeleopPeriodic() is run.

This might be related to the performance issue you are seeing.
Here is the "loop" I'm referring to...

void IterativeRobot::StartCompetition()
{
// first and one-time initialization
RobotInit();

// loop forever, calling the appropriate mode-dependent function
while (TRUE)
{
// Call the appropriate function depending upon the current robot mode
if (IsDisabled())
{
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if(!m_disabledInitialized)
{
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
DisabledPeriodic();
}
DisabledContinuous();
}
else if (IsAutonomous())
{
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if(!m_autonomousInitialized)
{
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
AutonomousPeriodic();
}
AutonomousContinuous();
}
else
{
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if(!m_teleopInitialized)
{
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
}
if (NextPeriodReady())
{
TeleopPeriodic();
}
TeleopContinuous();
}
}
}

Your code eliminated the default taskdelay() which ends up causing wasted CPU processing. The system should run smoother if you leave some time for valuable processing (in other tasks).

[duffany1]

Ahhh, I believe you may have been the guy I spoke to in person about this as we were gearing up for the final matches at WPI, trying to get our autonomous program working. We were happy to be a part of the winning alliance with you guys. By the way, we have a working autonomous at this point, though we can't score two tubes during autonomous as you can

To be honest, I really wish I had known about how to use the PID Controller before. The documentation either just seemed really shoddy (no offense to whoever wrote it) or I kind of didn't have enough knowledge at the time as to how I should be interpreting said documentation. At that point, one of our engineers just decided it'd be better to write it ourselves, but I guess it was good because I got to learn how PID actually works.

But in any case, if we ever have servo systems as complex as this, we will definitely be using PID Controller. It makes things so much easier... Although, homing / calibrating the machine still seems necessary, in case the pots somehow move. We have loads of gearing and our pots are kind of held in place by the good old piece of paper trick. So they tend to slip around when they are worked on.

Thanks for the other replies, too. This clarifies a LOT about PID!