NXC: custom PID control for NXT?
Posted: 21 Jan 2012, 10:44
I'm looking for custom PID functions written in NXC which could be included into own robot projects, because the Lego firmware PID control doesn't work accurately enough and reacts sometimes unexplainably unpredictable.
I found some code in the www ( http://libnxter.sourceforge.net/_p_i_d_8nxc.html ) but I'm not clear about how to use it:
Any other 100% NXC-compatible code is appreciated!
ps
does anybody know how to erase the line numbers?
I found some code in the www ( http://libnxter.sourceforge.net/_p_i_d_8nxc.html ) but I'm not clear about how to use it:
Code: Select all
/*
PID.nxc
Go to the documentation of this file
http://libnxter.sourceforge.net/_p_i_d_8nxc.html
*/
00001 /* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
00002 /*
00003 PID.nxc
00004 Copyright (C) 2008 Naba Kumar <[email protected]>
00005
00006 This program is free software; you can redistribute it and/or modify
00007 it under the terms of the GNU General Public License as published by
00008 the Free Software Foundation; either version 2 of the License, or
00009 (at your option) any later version.
00010
00011 This program is distributed in the hope that it will be useful,
00012 but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00014 GNU General Public License for more details.
00015
00016 You should have received a copy of the GNU General Public License
00017 along with this program; if not, write to the Free Software
00018 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00019 */
00020
00021 #ifndef _PID_CONTROLLER_H_
00022 #define _PID_CONTROLLER_H_
00023
00032 struct PIDControl
00033 {
00034 long PIDScale;
00035 long PValue;
00036 long IValue;
00037 long DValue;
00038 long setPoint;
00039 int steadyStateCountThreshold;
00043 /* Output parameters */
00044 long outputGain;
00045 long absMaxOutput;
00046 long absIntegralLimit;
00048 /* State */
00049 long integral;
00050 long lastError;
00051 long steadyStateCount;
00052 };
00053
00071 void PIDControlInit(PIDControl &pidControl, long PIDScale,
00072 long PValue, long IValue, long DValue,
00073 int steadyStateCountThreshold,
00074 long outputGain, long absMaxOutput)
00075 {
00076 pidControl.PIDScale = PIDScale;
00077 pidControl.PValue = PValue;
00078 pidControl.IValue = IValue;
00079 pidControl.DValue = DValue;
00080 pidControl.steadyStateCountThreshold = steadyStateCountThreshold;
00081 pidControl.outputGain = outputGain;
00082 pidControl.absMaxOutput = Abs(absMaxOutput);
00083 pidControl.absIntegralLimit = pidControl.absMaxOutput;
00084 pidControl.setPoint = 0;
00085 pidControl.integral = 0;
00086 pidControl.lastError = 0;
00087 pidControl.steadyStateCount = 0;
00088 }
00089
00094 void PIDControlSetIntegralLimit(PIDControl &pidControl, long absIntegralLimit)
00095 {
00096 pidControl.absIntegralLimit = Abs(absIntegralLimit);
00097 }
00098
00103 void PIDControlSetPoint(PIDControl &pidControl, long setPoint)
00104 {
00105 pidControl.setPoint = setPoint;
00106 pidControl.integral = 0;
00107 pidControl.steadyStateCount = 0;
00108 }
00109
00120 bool PIDControlCheckEnd(PIDControl &pidControl)
00121 {
00122 if (pidControl.steadyStateCount > pidControl.steadyStateCountThreshold)
00123 return true;
00124 else
00125 return false;
00126 }
00127
00131 long PIDControlGetLastError(PIDControl &pidControl)
00132 {
00133 return pidControl.lastError;
00134 }
00135
00142 long PIDControlStep(PIDControl &pidControl, long currentPoint)
00143 {
00144 long P, I, D, error, output;
00145
00146 if (PIDControlCheckEnd(pidControl))
00147 return 0;
00148
00149 error = pidControl.setPoint - currentPoint;
00150
00151 /* If error hasn't changed for last steadyStateCountThreshold samples, end PID control */
00152 if (pidControl.steadyStateCountThreshold > 0 && (error - pidControl.lastError) == 0)
00153 pidControl.steadyStateCount++;
00154
00155 P = ((pidControl.PValue * error) / pidControl.PIDScale);
00156 I = pidControl.integral + ((pidControl.IValue * error) / pidControl.PIDScale);
00157 D = ((pidControl.DValue) * (error - pidControl.lastError)) / pidControl.PIDScale;
00158
00159 /* Limit integral output */
00160 if (I > (pidControl.absIntegralLimit/pidControl.outputGain))
00161 I = pidControl.absIntegralLimit;
00162 if (I < -(pidControl.absIntegralLimit/pidControl.outputGain))
00163 I = -pidControl.absIntegralLimit;
00164
00165 output = pidControl.outputGain * (P + I + D);
00166 if (output > pidControl.absMaxOutput) output = pidControl.absMaxOutput;
00167 if (output < -pidControl.absMaxOutput) output = -pidControl.absMaxOutput;
00168
00169 pidControl.integral = I;
00170 pidControl.lastError = error;
00171
00172 return output;
00173 }
00174
00175 #endif
00176
// Generated on Sat Jun 6 12:50:27 2009 for libnxter by doxygen 1.5.6
ps
does anybody know how to erase the line numbers?