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);