[jdong]

This code could be used in any feedback loop. I wrote it all in theory, using a simple simulator for testing... Robot still not back from FedEx yet....


Testing is well appreciated. Feel free to use if desired.


--NOTE-- The following source was designed to run on a PC (Actually, Cygwin+KDevelop). Tweak accordingly. You only need the do_discrete_control and its global variables on the robot -- the rest was for PC simulation.

/*
 *
 *	Proportional-Integral-Derivitive Closed-Loop feedback algorithm.
 *
 *
 *	By John Dong, Team 245 (Adam-Bots)
 *
 *	jdong@wideopenwest.com
 *
 *	www.adamsrobotics.com
 *
 *
 *
 */



#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/* These keep track of previous errors */

int error=0, Lerror=0, LLerror=0;
int last_out=0;




/*THIS is the bulk of the program. Pass in actual and wanted values,
 * and get back the optimal PWM value.
 *
 *
 * Constants to tweak are kp, ki, and kd, for each of the 3 bands.
 *
 * Set a constant to ZERO in order to disable the band.
 *
 * 	(You may want to disable the derivative band, as it's just
 * 	 a dampener. May not be worth calculation cost)
 *
 *
 */

int do_discrete_control(int actual, int wanted, int kp, int ki , int kd)
{
  int out; //Store output.
  LLerror=Lerror;
  Lerror=error;
  error=wanted-actual;

  out=last_out+kp*(error-Lerror)+ki*error+kd*(error-2*Lerror+LLerror);

  if(out>127) out=127;
  if(out<-127) out=-127;
  return out;
}


#define KP 		4

#define KI 		5

#define KD		0


int main(int argc, char *argv[])
{
  int i=0;

        int pwm,pos;
	pwm=0; pos=0;
  for(i=0;i<600;i++)
  {
    pwm=do_discrete_control(pos,250,KP,KI,KD);
    pos+=(double)pwm*.1;
    printf("# %d\t\tPOS: %d\t\tPWM:%d\n",i,pos,pwm);
    if(i==500)
    {
    	scanf("%d",&pos);
        i=0;
    }
  }
  getchar();
  return EXIT_SUCCESS;
}