modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c

   1 /*
   2  *  Copyright Droids Corporation (2007)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: blocking_detection_manager.c,v 1.1.2.7 2009-05-18 12:29:10 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23 /* blocking detection manager */
  24
  25 #include <stdio.h>
  26 #include <string.h>
  27 #include <aversive/error.h>
  28
  29 #include <blocking_detection_manager.h>
  30
  31 /** init module, give the robot system to use as a parameter */
  32 void bd_init(struct blocking_detection * bd)
  33 {
  34         uint8_t flags;
  35         IRQ_LOCK(flags);
  36         memset(bd, 0, sizeof(*bd));
  37         IRQ_UNLOCK(flags);
  38 }
  39
  40 /* thresholds */
  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)
  44 {
  45         uint8_t flags;
  46         IRQ_LOCK(flags);
  47         bd->k1 = k1;
  48         bd->k2 = k2;
  49         bd->i_thres = i_thres;
  50         bd->cpt_thres = cpt_thres;
  51         bd->cpt = 0;
  52         IRQ_UNLOCK(flags);
  53 }
  54
  55 /* speed threshold */
  56 void bd_set_speed_threshold(struct blocking_detection * bd,
  57                             uint16_t speed)
  58 {
  59         uint8_t flags;
  60         IRQ_LOCK(flags);
  61         bd->speed_thres = speed;
  62         IRQ_UNLOCK(flags);
  63 }
  64
  65 /** reset current blocking */
  66 void bd_reset(struct blocking_detection * bd)
  67 {
  68         uint8_t flags;
  69         IRQ_LOCK(flags);
  70         bd->cpt = 0;
  71         IRQ_UNLOCK(flags);
  72 }
  73
  74
  75
  76 /** function to be called periodically */
  77 void bd_manage_from_speed_cmd(struct blocking_detection * bd,
  78                               int32_t speed, int32_t cmd)
  79 {
  80         int32_t i=0;
  81
  82         /* if current-based blocking_detection enabled */
  83         if ( bd->cpt_thres ) {
  84                 i = bd->k1 * cmd - bd->k2 * speed;
  85                 if ((uint32_t)ABS(i) > bd->i_thres &&
  86                     (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
  87                         if (bd->cpt == bd->cpt_thres - 1)
  88                                 WARNING(E_BLOCKING_DETECTION_MANAGER,
  89                                       "BLOCKING cmd=%ld, speed=%ld i=%ld",
  90                                       cmd, speed, i);
  91                         if (bd->cpt < bd->cpt_thres)
  92                                 bd->cpt++;
  93                 }
  94                 else {
  95                         bd->cpt=0;
  96                 }
  97 #if BD_DEBUG
  98                 if (bd->debug_cpt++ == BD_DEBUG) {
  99                         DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
 100                               cmd, speed, i);
 101                         bd->debug_cpt = 0;
 102                 }
 103         }
 104 #endif
 105 }
 106
 107 /** function to be called periodically */
 108 void bd_manage_from_pos_cmd(struct blocking_detection * bd,
 109                             int32_t pos, int32_t cmd)
 110 {
 111         int32_t speed = (pos - bd->prev_pos);
 112         bd_manage_from_speed_cmd(bd, speed, cmd);
 113         bd->prev_pos = pos;
 114 }
 115
 116 /** function to be called periodically */
 117 void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
 118 {
 119         bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs), cs_get_out(cs));
 120 }
 121
 122 /** get value of blocking detection */
 123 uint8_t bd_get(struct blocking_detection * bd)
 124 {
 125         uint8_t ret, flags;
 126         IRQ_LOCK(flags);
 127         ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));
 128         IRQ_UNLOCK(flags);
 129         return ret;
 130 }