Commit from zer0 on branch b_zer0 (2007-12-05 19:08 CET)
=================================
Add ability to filter the derivate term in the PID
aversive config/Configure.help
1.13.4.11
aversive config/config.in
1.42.4.13
aversive modules/devices/control_system/filters/pid/pid.c
1.5.4.4
aversive modules/devices/control_system/filters/pid/pid.h
1.4.4.5
aversive modules/devices/control_system/filters/pid/config/pid_config.h
1.1.10.2
aversive modules/devices/control_system/filters/pid/test/.config
1.7.4.8
aversive modules/devices/control_system/filters/pid/test/main.c
1.4.6.3
aversive modules/devices/control_system/filters/pid/test/pid_config.h
1.1.10.2
==============================
aversive/config/Configure.help (1.13.4.10 -> 1.13.4.11)
==============================
@@ -232,6 +232,10 @@
CONFIG_MODULE_PID
This filter provides a PID (proportionnal, integral, derivate).
+CONFIG_MODULE_PID_CREATE_CONFIG
+ Create a pid_config.h file if it does not exist, with a default
+ configuration.
+
CONFIG_MODULE_RAMP
This module limits the variation of the input of the filter. It can
be used in a speed control system as a consign filter to set a
=========================
aversive/config/config.in (1.42.4.12 -> 1.42.4.13)
=========================
@@ -328,6 +328,9 @@
bool 'PID' CONFIG_MODULE_PID
+dep_bool ' |-- Create Default PID config' CONFIG_MODULE_PID_CREATE_CONFIG \
+ $CONFIG_MODULE_PID
+
bool 'ramp' CONFIG_MODULE_RAMP
bool 'Quadramp' CONFIG_MODULE_QUADRAMP
=========================================================
aversive/modules/devices/control_system/filters/pid/pid.c (1.5.4.3 -> 1.5.4.4)
=========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pid.c,v 1.5.4.3 2007-12-04 09:29:25 zer0 Exp $
+ * Revision : $Id: pid.c,v 1.5.4.4 2007-12-05 18:08:39 zer0 Exp $
*
*/
@@ -30,6 +30,7 @@
IRQ_LOCK(flags);
memset(p, 0, sizeof(*p));
p->gain_P = 1 ;
+ p->derivate_nb_samples = 1;
IRQ_UNLOCK(flags);
}
@@ -61,6 +62,22 @@
IRQ_UNLOCK(flags);
}
+int8_t pid_set_derivate_filter(struct pid_filter *p, uint8_t nb_samples)
+{
+ uint8_t flags;
+ int8_t ret;
+ IRQ_LOCK(flags);
+ if (nb_samples > PID_DERIVATE_FILTER_MAX_SIZE) {
+ ret = -1;
+ }
+ else {
+ p->derivate_nb_samples = nb_samples;
+ ret = 0;
+ }
+ IRQ_UNLOCK(flags);
+ return ret;
+}
+
int16_t pid_get_gain_P(struct pid_filter *p)
{
return (p->gain_P);
@@ -98,6 +115,11 @@
return (p->out_shift);
}
+uint8_t pid_get_derivate_filter(struct pid_filter *p)
+{
+ return (p->derivate_nb_samples);
+}
+
int32_t pid_get_value_I(struct pid_filter *p)
{
uint8_t flags;
@@ -113,7 +135,7 @@
uint8_t flags;
int32_t ret;
IRQ_LOCK(flags);
- ret = (p->prev_in);
+ ret = p->prev_samples[p->index];
IRQ_UNLOCK(flags);
return ret;
}
@@ -123,7 +145,7 @@
uint8_t flags;
int32_t ret;
IRQ_LOCK(flags);
- ret = (p->prev_D);
+ ret = p->prev_D;
IRQ_UNLOCK(flags);
return ret;
}
@@ -144,6 +166,7 @@
int32_t derivate ;
int32_t command ;
struct pid_filter * p = data;
+ uint8_t prev_index;
/*
* Integral value : the integral become bigger with time .. (think
@@ -151,12 +174,19 @@
* integral = previous integral + current value
*/
- /* derivate value */
- /* f(t+h) - f(t) with f(t+h) = current value */
- /* derivate = ------------- f(t) = previous value */
- /* h */
- /* so derivate = current error - previous error */
- derivate = in - p->prev_in ;
+ /* derivate value
+ * f(t+h) - f(t) with f(t+h) = current value
+ * derivate = ------------- f(t) = previous value
+ * h
+ * so derivate = current error - previous error
+ *
+ * We can apply a filter to reduce noise on the derivate term,
+ * by using a bigger period.
+ */
+
+ prev_index = p->index + 1;
+ prev_index %= p->derivate_nb_samples;
+ derivate = in - p->prev_samples[prev_index];
/* saturate input... it influences integral */
if (p->max_in)
@@ -182,9 +212,11 @@
/* backup of current error value (for the next calcul of derivate
value) */
- p->prev_in = in ;
+ p->prev_samples[p->index] = in ;
+ p->index ++;
+ p->index %= p->derivate_nb_samples;
p->prev_D = derivate ;
p->prev_out = command ;
-
+
return command;
}
=========================================================
aversive/modules/devices/control_system/filters/pid/pid.h (1.4.4.4 -> 1.4.4.5)
=========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pid.h,v 1.4.4.4 2007-12-04 09:29:25 zer0 Exp $
+ * Revision : $Id: pid.h,v 1.4.4.5 2007-12-05 18:08:39 zer0 Exp $
*
*/
@@ -25,23 +25,29 @@
#include <aversive.h>
#include <stdlib.h>
+#include <pid_config.h>
+
+
/** this is the pid_filter structure*/
struct pid_filter
{
- int16_t gain_P; /**< Gain of Proportionnal module */
- int16_t gain_I; /**< Gain of Integral module */
- int16_t gain_D; /**< Gain of Derivate module */
-
- uint8_t out_shift;
-
- int32_t max_in; /**< In saturation levels */
- int32_t max_I; /**< Integral saturation levels */
- int32_t max_out; /**< Out saturation levels */
-
- int32_t integral; /**< previous integral parameter */
- int32_t prev_in; /**< previous in */
- int32_t prev_D; /**< previous derivate parameter */
- int32_t prev_out; /**< previous out command (for debug only) */
+ int16_t gain_P; /**< Gain of Proportionnal module */
+ int16_t gain_I; /**< Gain of Integral module */
+ int16_t gain_D; /**< Gain of Derivate module */
+
+ uint8_t out_shift;
+
+ uint8_t derivate_nb_samples; /**< sample count for derivate filter */
+ uint8_t index; /**< index in circular buffer below */
+ int32_t prev_samples[PID_DERIVATE_FILTER_MAX_SIZE]; /**< previous in
(circular buf) */
+
+ int32_t max_in; /**< In saturation levels */
+ int32_t max_I; /**< Integral saturation levels */
+ int32_t max_out; /**< Out saturation levels */
+
+ int32_t integral; /**< previous integral parameter */
+ int32_t prev_D; /**< previous derivate parameter */
+ int32_t prev_out; /**< previous out command (for debug only) */
};
/** Prototyping */
@@ -52,6 +58,7 @@
void pid_set_gains(struct pid_filter *p, int16_t gp, int16_t gi, int16_t gd) ;
void pid_set_maximums(struct pid_filter *p, int32_t max_in, int32_t max_I,
int32_t max_out);
void pid_set_out_shift(struct pid_filter *p, uint8_t out_shift);
+int8_t pid_set_derivate_filter(struct pid_filter *p, uint8_t nb_samples);
/* accessors of all parameter of pid structure*/
int16_t pid_get_gain_P(struct pid_filter *p);
@@ -61,6 +68,7 @@
int32_t pid_get_max_I(struct pid_filter *p);
int32_t pid_get_max_out(struct pid_filter *p);
uint8_t pid_get_out_shift(struct pid_filter *p);
+uint8_t pid_get_derivate_filter(struct pid_filter *p);
int32_t pid_get_value_I(struct pid_filter *p);
int32_t pid_get_value_in(struct pid_filter *p);
=======================================================================
aversive/modules/devices/control_system/filters/pid/config/pid_config.h
(1.1.10.1 -> 1.1.10.2)
=======================================================================
@@ -21,4 +21,10 @@
#ifndef PID_CONFIG_H
#define PID_CONFIG_H
+
+/** the derivate term can be filtered to remove the noise. This value
+ * is the maxium sample count to keep in memory to do this
+ * filtering. For an instance of pid, this count is defined o*/
+#define PID_DERIVATE_FILTER_MAX_SIZE 4
+
#endif
================================================================
aversive/modules/devices/control_system/filters/pid/test/.config (1.7.4.7 ->
1.7.4.8)
================================================================
@@ -80,7 +80,9 @@
# CONFIG_MODULE_VECT2 is not set
# CONFIG_MODULE_SCHEDULER is not set
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
-# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+CONFIG_MODULE_SCHEDULER_USE_TIMERS=y
+# CONFIG_MODULE_SCHEDULER_TIMER0 is not set
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
# CONFIG_MODULE_TIME is not set
# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
@@ -164,6 +166,7 @@
# Filters
#
CONFIG_MODULE_PID=y
+CONFIG_MODULE_PID_CREATE_CONFIG=y
# CONFIG_MODULE_RAMP is not set
# CONFIG_MODULE_QUADRAMP is not set
# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
===============================================================
aversive/modules/devices/control_system/filters/pid/test/main.c (1.4.6.2 ->
1.4.6.3)
===============================================================
@@ -15,20 +15,37 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.c,v 1.4.6.2 2007-05-23 17:18:13 zer0 Exp $
+ * Revision : $Id: main.c,v 1.4.6.3 2007-12-05 18:08:40 zer0 Exp $
*
*/
-#include <pid.h>
-#include <aversive/wait.h>
+#include <stdio.h>
+
+#include <pid.h>
+int32_t tab[] = { 0, 1, 2, 5, 6, 0, 0, 1, 0, 0, -10, -8, -5, -1 };
int main(void)
{
- while(1) {
- wait_ms(1000);
- }
- return 0;
+ struct pid_filter p;
+ int32_t val;
+ int i;
+
+ pid_init(&p);
+ pid_set_gains(&p, 1000, 30, 5000);
+ pid_set_maximums(&p, 0, 50000, 4095);
+ pid_set_out_shift(&p, 10);
+ pid_set_derivate_filter(&p, 4);
+
+ for (i=0; i < (sizeof(tab)/sizeof(int32_t)); i++) {
+ val = pid_do_filter(&p, tab[i]);
+ printf("in = %" PRId32 "\n", tab[i]);
+ printf("out = %" PRId32 "\n", val);
+ printf("I = %" PRId32 "\n", p.integral);
+ printf("D = %" PRId32 "\n\n", p.prev_D);
+ }
+
+ return 0;
}
=====================================================================
aversive/modules/devices/control_system/filters/pid/test/pid_config.h
(1.1.10.1 -> 1.1.10.2)
=====================================================================
@@ -18,3 +18,13 @@
*
*
*/
+
+#ifndef PID_CONFIG_H
+#define PID_CONFIG_H
+
+/** the derivate term can be filtered to remove the noise. This value
+ * is the maxium sample count to keep in memory to do this
+ * filtering. For an instance of pid, this count is defined o*/
+#define PID_DERIVATE_FILTER_MAX_SIZE 4
+
+#endif
Commit from zer0 (2007-12-05 19:08 CET)
================
Add ability to filter the derivate term in the PID
aversive_projects microb2008/main/.config 1.10
aversive_projects microb2008/main/main.c 1.11
+ aversive_projects microb2008/main/pid_config.h 1.1
=========================================
aversive_projects/microb2008/main/.config (1.9 -> 1.10)
=========================================
@@ -154,6 +154,7 @@
#
CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
CONFIG_MODULE_PID=y
+CONFIG_MODULE_PID_CREATE_CONFIG=y
# CONFIG_MODULE_RAMP is not set
CONFIG_MODULE_QUADRAMP=y
# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
========================================
aversive_projects/microb2008/main/main.c (1.10 -> 1.11)
========================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.c,v 1.10 2007-12-04 09:38:52 zer0 Exp $
+ * Revision : $Id: main.c,v 1.11 2007-12-05 18:08:40 zer0 Exp $
*
*/
@@ -437,9 +437,10 @@
/* CS DISTANCE */
pid_init(&robot.pid_d);
- pid_set_gains(&robot.pid_d, 1000, 30, 20000);
+ pid_set_gains(&robot.pid_d, 1000, 30, 5000);
pid_set_maximums(&robot.pid_d, 0, 50000, 4095);
pid_set_out_shift(&robot.pid_d, 10);
+ pid_set_derivate_filter(&robot.pid_d, 4);
quadramp_init(&robot.qr_d);
quadramp_set_1st_order_vars(&robot.qr_d, 2000, 2000); /* set speed */
@@ -454,9 +455,10 @@
/* CS ANGLE */
pid_init(&robot.pid_a);
- pid_set_gains(&robot.pid_a, 1000, 15, 20000);
+ pid_set_gains(&robot.pid_a, 1000, 15, 5000);
pid_set_maximums(&robot.pid_a, 0, 100000, 4095);
pid_set_out_shift(&robot.pid_a, 10);
+ pid_set_derivate_filter(&robot.pid_a, 4);
quadramp_init(&robot.qr_a);
quadramp_set_1st_order_vars(&robot.qr_a, 2000, 2000); /* set speed */
@@ -529,24 +531,24 @@
printf_P(PSTR(" **** .****. \r\n"));
printf_P(PSTR(" .*....****** .*****.....*. \r\n"));
printf_P(PSTR(" ..**,. .:**.. \r\n"));
- printf_P(PSTR("Please wait ... "));
- printf_P(PSTR("Bitte Schnell Edition 3.0 : Um zu gut zu machen."));
+ printf_P(PSTR("Bitte Schnell Edition 3.0"));
+ printf_P(PSTR(" Um zu gut zu machen."));
for (i = 0; i < 20; i++) {
printf_P(PSTR("\b|"));
- wait_ms(5);
+ wait_ms(3);
printf_P(PSTR("\b/"));
- wait_ms(4);
+ wait_ms(3);
printf_P(PSTR("\b-"));
wait_ms(3);
printf_P(PSTR("\b.\\"));
- wait_ms(2);
+ wait_ms(3);
}
-
- rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
wait_ms(100);
printf_P("\n");
+
+ rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
snprintf(prompt, sizeof(prompt), "microb > ");
rdline_newline(&rdl, prompt);
==============================================
aversive_projects/microb2008/main/pid_config.h (1.1)
==============================================
@@ -0,0 +1,30 @@
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ *
+ *
+ */
+
+#ifndef PID_CONFIG_H
+#define PID_CONFIG_H
+
+/** the derivate term can be filtered to remove the noise. This value
+ * is the maxium sample count to keep in memory to do this
+ * filtering. For an instance of pid, this count is defined o*/
+#define PID_DERIVATE_FILTER_MAX_SIZE 4
+
+#endif
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
