2 * Copyright Droids Corporation (2007)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: blocking_detection_manager.c,v 1.1.2.7 2009-05-18 12:29:10 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
23 /* blocking detection manager */
27 #include <aversive/error.h>
29 #include <blocking_detection_manager.h>
31 /** init module, give the robot system to use as a parameter */
32 void bd_init(struct blocking_detection * bd)
36 memset(bd, 0, sizeof(*bd));
41 void bd_set_current_thresholds(struct blocking_detection * bd,
42 int32_t k1, int32_t k2,
43 uint32_t i_thres, uint16_t cpt_thres)
49 bd->i_thres = i_thres;
50 bd->cpt_thres = cpt_thres;
56 void bd_set_speed_threshold(struct blocking_detection * bd,
61 bd->speed_thres = speed;
65 /** reset current blocking */
66 void bd_reset(struct blocking_detection * bd)
74 static inline uint8_t same_sign(int32_t a, int32_t b)
83 /** function to be called periodically */
84 void bd_manage_from_speed_cmd(struct blocking_detection * bd,
85 int32_t speed, int32_t cmd)
90 if (bd->cpt_thres == 0)
93 i = bd->k1 * cmd - bd->k2 * speed;
95 /* if i is above threshold, speed is below threshold, and cmd
96 * has the same sign than i */
97 if ((uint32_t)ABS(i) > bd->i_thres &&
98 (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
100 if (bd->cpt == bd->cpt_thres - 1)
101 WARNING(E_BLOCKING_DETECTION_MANAGER,
102 "BLOCKING cmd=%ld, speed=%ld i=%ld",
104 if (bd->cpt < bd->cpt_thres)
111 if (bd->debug_cpt++ == BD_DEBUG) {
112 DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
119 /** function to be called periodically */
120 void bd_manage_from_pos_cmd(struct blocking_detection * bd,
121 int32_t pos, int32_t cmd)
123 int32_t speed = (pos - bd->prev_pos);
124 bd_manage_from_speed_cmd(bd, speed, cmd);
128 /** function to be called periodically */
129 void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
131 bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs), cs_get_out(cs));
134 /** get value of blocking detection */
135 uint8_t bd_get(struct blocking_detection * bd)
139 ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));