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 static inline  uint8_t same_sign(int32_t a, int32_t b)
  75 {
  76         if (a >= 0 && b >= 0)
  77                 return 1;
  78         if (a < 0 && b < 0)
  79                 return 1;
  80         return 0;
  81 }
  82
  83 /** function to be called periodically */
  84 void bd_manage_from_speed_cmd(struct blocking_detection * bd,
  85                               int32_t speed, int32_t cmd)
  86 {
  87         int32_t i = 0;
  88
  89         /* disabled */
  90         if (bd->cpt_thres == 0)
  91                 return;
  92
  93         i = bd->k1 * cmd - bd->k2 * speed;
  94
  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) &&
  99             same_sign(i, cmd)) {
 100                 if (bd->cpt == bd->cpt_thres - 1)
 101                         WARNING(E_BLOCKING_DETECTION_MANAGER,
 102                                 "BLOCKING cmd=%ld, speed=%ld i=%ld",
 103                                 cmd, speed, i);
 104                 if (bd->cpt < bd->cpt_thres)
 105                         bd->cpt++;
 106         }
 107         else {
 108                 bd->cpt=0;
 109         }
 110 #if BD_DEBUG
 111         if (bd->debug_cpt++ == BD_DEBUG) {
 112                 DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
 113                       cmd, speed, i);
 114                 bd->debug_cpt = 0;
 115         }
 116 #endif
 117 }
 118
 119 /** function to be called periodically */
 120 void bd_manage_from_pos_cmd(struct blocking_detection * bd,
 121                             int32_t pos, int32_t cmd)
 122 {
 123         int32_t speed = (pos - bd->prev_pos);
 124         bd_manage_from_speed_cmd(bd, speed, cmd);
 125         bd->prev_pos = pos;
 126 }
 127
 128 /** function to be called periodically */
 129 void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
 130 {
 131         bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs), cs_get_out(cs));
 132 }
 133
 134 /** get value of blocking detection */
 135 uint8_t bd_get(struct blocking_detection * bd)
 136 {
 137         uint8_t ret, flags;
 138         IRQ_LOCK(flags);
 139         ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));
 140         IRQ_UNLOCK(flags);
 141         return ret;
 142 }