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 /** function to be called periodically */
  75 void bd_manage_from_speed_cmd(struct blocking_detection * bd,
  76                               int32_t speed, int32_t cmd)
  77 {
  78         int32_t i = 0;
  79
  80         /* disabled */
  81         if (bd->cpt_thres == 0)
  82                 return;
  83
  84         i = bd->k1 * cmd - bd->k2 * speed;
  85
  86         /* if i is above threshold, speed is below threshold, and cmd
  87          * has the same sign than i */
  88         if ((uint32_t)ABS(i) > bd->i_thres &&
  89             (bd->speed_thres == 0 || ABS(speed) < bd->speed_thres) &&
  90             (i * cmd > 0)) {
  91                 if (bd->cpt == bd->cpt_thres - 1)
  92                         WARNING(E_BLOCKING_DETECTION_MANAGER,
  93                                 "BLOCKING cmd=%ld, speed=%ld i=%ld",
  94                                 cmd, speed, i);
  95                 if (bd->cpt < bd->cpt_thres)
  96                         bd->cpt++;
  97         }
  98         else {
  99                 bd->cpt=0;
 100         }
 101 #if BD_DEBUG
 102         if (bd->debug_cpt++ == BD_DEBUG) {
 103                 DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld i=%ld",
 104                       cmd, speed, i);
 105                 bd->debug_cpt = 0;
 106         }
 107 #endif
 108 }
 109
 110 /** function to be called periodically */
 111 void bd_manage_from_pos_cmd(struct blocking_detection * bd,
 112                             int32_t pos, int32_t cmd)
 113 {
 114         int32_t speed = (pos - bd->prev_pos);
 115         bd_manage_from_speed_cmd(bd, speed, cmd);
 116         bd->prev_pos = pos;
 117 }
 118
 119 /** function to be called periodically */
 120 void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
 121 {
 122         bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs), cs_get_out(cs));
 123 }
 124
 125 /** get value of blocking detection */
 126 uint8_t bd_get(struct blocking_detection * bd)
 127 {
 128         uint8_t ret, flags;
 129         IRQ_LOCK(flags);
 130         ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));
 131         IRQ_UNLOCK(flags);
 132         return ret;
 133 }