bertos/drv/dc_motor.c

   1 /**
   2  * \file
   3  * <!--
   4  * This file is part of BeRTOS.
   5  *
   6  * Bertos is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  * As a special exception, you may use this file as part of a free software
  21  * library without restriction.  Specifically, if other files instantiate
  22  * templates or use macros or inline functions from this file, or you compile
  23  * this file and link it with other files to produce an executable, this
  24  * file does not by itself cause the resulting executable to be covered by
  25  * the GNU General Public License.  This exception does not however
  26  * invalidate any other reasons why the executable file might be covered by
  27  * the GNU General Public License.
  28  *
  29  * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
  30  * -->
  31  *
  32  *
  33  * \brief DC motor driver (implementation)
  34  *
  35  * Thi module provide a simple api to controll a DC motor in direction and
  36  * speed, to allow this we use a  Back-EMF technique.
  37  * This technique is basid on the capability of the DC motor to became generator
  38  * of voltage when we turn off its supply. This happend every time we turn off the
  39  * DC motor supply, and it continue to rotate for a short time thanks its mechanical
  40  * energy. Using this idea we can turn off the motor for a very short time, and
  41  * going to read the volage value from DC motor supply pins. This voltage say to us
  42  * the actual speed of the motor.
  43  * Sampling the DC motor speed we are able to controll its speed.
  44  *
  45  * \author Daniele Basile <asterix@develer.com>
  46  */
  47
  48 #include "dc_motor.h"
  49 #include "hw/hw_dc_motor.h"
  50
  51 // Define logging setting (for cfg/log.h module).
  52 #define LOG_LEVEL         DC_MOTOR_LOG_LEVEL
  53 #define LOG_VERBOSITY     DC_MOTOR_LOG_FORMAT
  54
  55 #include <cfg/log.h>
  56 #include <cfg/debug.h>
  57
  58 #include <algo/pid_control.h>
  59
  60 #include <drv/timer.h>
  61
  62 #include <kern/proc.h>
  63
  64 #include <string.h>
  65
  66 /**
  67  * Define status bit for DC motor device.
  68  */
  69 #define DC_MOTOR_ACTIVE           BV(0)     ///< DC motor enable or disable flag.
  70 #define DC_MOTOR_DIR              BV(1)     ///< Spin direction of DC motor.
  71
  72 /*
  73  * Some utility macro for motor directions
  74  */
  75 #define POS_DIR                   1
  76 #define NEG_DIR                   0
  77 #define DC_MOTOR_POS_DIR(x)       ((x) |= DC_MOTOR_DIR)   // Set directions status positive
  78 #define DC_MOTOR_NEG_DIR(x)       ((x) &= ~DC_MOTOR_DIR)  // Set directions status negative
  79
  80 // Update the status with current direction
  81 #define DC_MOTOR_SET_STATUS_DIR(status, dir) \
  82                 (dir == POS_DIR ? DC_MOTOR_POS_DIR(status) : DC_MOTOR_NEG_DIR(status))
  83
  84
  85 /**
  86  * DC motor definition.
  87  */
  88 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
  89 static int dcm_registered_num;
  90
  91 // Stack process for DC motor poll.
  92 static PROC_DEFINE_STACK(dc_motor_poll_stack, 400);
  93
  94 // Sample period for all DC motor.
  95 static mtime_t sample_period;
  96
  97 // Only for Debug
  98 LOG_INFOB(static int debug_msg_delay = 0;);
  99
 100
 101 INLINE dc_speed_t dc_motor_readSpeed(int index)
 102 {
 103         DCMotor *dcm = &dcm_all[index];
 104         LOG_INFO("DC motor[%d]\n", index);
 105
 106         return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max);
 107 }
 108
 109 /**
 110  * Read the target speed from select device.
 111  */
 112 dc_speed_t dc_motor_readTargetSpeed(int index)
 113 {
 114         DCMotor *dcm = &dcm_all[index];
 115         LOG_INFO("DC motor[%d]\n", index);
 116
 117         return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED);
 118
 119 }
 120
 121
 122 /*
 123  * Sampling a signal on DC motor and compute
 124  * a new value of speed according with PID control.
 125  */
 126 static void dc_motor_do(int index)
 127 {
 128         DCMotor *dcm = &dcm_all[index];
 129
 130         dc_speed_t curr_pos = 0;
 131         pwm_duty_t new_pid;
 132
 133         //If select DC motor is not active we return
 134         if (!(dcm->status & DC_MOTOR_ACTIVE))
 135                 return;
 136
 137
 138         //Compute next value for reaching target speed from current position
 139         if (dcm->cfg->pid_enable)
 140         {
 141                 curr_pos = dc_motor_readSpeed(index);
 142                 new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
 143         }
 144         else
 145                 new_pid = dcm->tgt_speed;
 146
 147         LOG_INFOB(if (debug_msg_delay == 20)
 148         {
 149                 LOG_INFO("DC Motor[%d]: curr_speed[%d],curr_pos[%d],tgt[%d]\n", dcm->index,
 150                                                         curr_pos, curr_pos, dcm->tgt_speed);
 151                 debug_msg_delay = 0;
 152         }
 153         debug_msg_delay++;
 154         kputs("\n"););
 155
 156         //Apply the compute duty value
 157         pwm_setDuty(dcm->cfg->pwm_dev, new_pid);
 158
 159         //Restart dc motor
 160         pwm_enable(dcm->cfg->pwm_dev, true);
 161
 162         DC_MOTOR_ENABLE(dcm->index);
 163 }
 164
 165
 166 /*
 167  * Check if the DC motor run time is expired, if this happend
 168  * we turn off motor and reset status.
 169  */
 170 INLINE bool check_timerIsExpired(int index)
 171 {
 172         if (((dcm_all[index].expire_time - timer_clock()) < 0) &&
 173                         (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE))
 174         {
 175                 dc_motor_enable(index, false, DC_MOTOR_IDLE_MODE);
 176                 dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
 177                 return false;
 178         }
 179
 180         return true;
 181 }
 182
 183 /**
 184  * Process to poll DC motor status.
 185  * To use a Back-EMF technique (see brief for more details),
 186  * we turn off a motor for max_sample_delay, that value are stored
 187  * in each DC motor config. For this implementation we assume
 188  * that have a common max_sample_delay, choose among a max delay
 189  * to all DC motor configuration.
 190  * The DC motor off time is choose to allow the out signal to
 191  * be stable, so we can read and process this value for feedback
 192  * controll loop.
 193  * The period (sample_period - max_sample_delay) that every time
 194  * we turn off a DC motor is choose to have a feedback controll
 195  * more responsive or less responsive.
 196  */
 197 static void NORETURN dc_motor_poll(void)
 198 {
 199         for (;;)
 200         {
 201                 mtime_t max_sample_delay = 0;
 202                 int i;
 203
 204                 /*
 205                  * For all DC motor we read and process output singal,
 206                  * and choose the max value to off time
 207                  */
 208                 for (i = 0; i < dcm_registered_num; i++)
 209                 {
 210                         if (check_timerIsExpired(i))
 211                         {
 212                                 dc_motor_do(i);
 213                                 max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay);
 214                         }
 215
 216                         /*
 217                          * If we read speed from trimmer we update the target
 218                          * speed value when motor is running so we can make
 219                          * dc motor speed regulation.
 220                          */
 221                         if (dcm_all[i].cfg->enable_dev_speed)
 222                                 dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i));
 223                 }
 224
 225                 //Wait for next sampling
 226                 timer_delay(sample_period - max_sample_delay);
 227
 228                 for (i = 0; i < dcm_registered_num; i++)
 229                 {
 230                         check_timerIsExpired(i);
 231
 232                         if (dcm_all[i].status & DC_MOTOR_ACTIVE)
 233                         {
 234                                 DC_MOTOR_DISABLE(dcm_all[i].index);
 235                                 pwm_enable(dcm_all[i].cfg->pwm_dev, false);
 236                         }
 237                 }
 238
 239                 //Wait some time to allow signal to stabilize before sampling
 240                 timer_delay(max_sample_delay);
 241         }
 242 }
 243
 244 /**
 245  * Set spin direction of DC motor.
 246  *
 247  * \a index number of DC motor
 248  * \a dir direction of DC motor
 249  */
 250 void dc_motor_setDir(int index, bool dir)
 251 {
 252         DCMotor *dcm = &dcm_all[index];
 253
 254         /*
 255          * To set dc motor direction we must also set the
 256          * PWM polarity according with dc motor driver chip
 257          */
 258         pwm_setPolarity(dcm->cfg->pwm_dev, dir);
 259         DC_MOTOR_SET_DIR(dcm->index, dir);
 260         DC_MOTOR_SET_STATUS_DIR(dcm->status, dir);
 261 }
 262
 263
 264 /**
 265  * Set DC motor speed.
 266  */
 267 void dc_motor_setSpeed(int index, dc_speed_t speed)
 268 {
 269         DCMotor *dcm = &dcm_all[index];
 270
 271         dcm->tgt_speed = speed;
 272
 273         LOG_INFO("DC Motor[%d]: Tspeed[%d]\n", index, dcm->tgt_speed);
 274 }
 275
 276 /**
 277  * Set among of time that dc motor should run.
 278  */
 279  void dc_motor_setTimer(int index, mtime_t on_time)
 280  {
 281          dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
 282          if (on_time != DC_MOTOR_NO_EXPIRE)
 283                 dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time);
 284  }
 285
 286 /**
 287  * Enable or disable dc motor.
 288  *
 289  * There are two \a mode to disable the dc motor:
 290  *  - DC_MOTOR_DISABLE_MODE
 291  *  - DC_MOTOR_IDLE
 292  *
 293  * The DC_MOTOR_DISABLE_MODE shut down the DC motor and
 294  * leave it floating to rotate.
 295  * The DC_MOTOR_IDLE does not shut down DC motor, but put
 296  * its supply pin in short circuite, in this way the motor result
 297  * braked from intentional rotation.
 298  */
 299 void dc_motor_enable(int index, bool state, int mode)
 300 {
 301         DCMotor *dcm = &dcm_all[index];
 302
 303         /*
 304          * Clean all PID stutus variable, becouse
 305          * we start with new one.
 306          */
 307         pid_control_reset(&dcm->pid_ctx);
 308
 309         if (state)
 310         {
 311                 dcm->status |= DC_MOTOR_ACTIVE;
 312         }
 313         else
 314         {
 315                 pwm_enable(dcm->cfg->pwm_dev, false);
 316                 dcm->status &= ~DC_MOTOR_ACTIVE;
 317
 318                 if (mode == DC_MOTOR_DISABLE_MODE)
 319                         DC_MOTOR_DISABLE(dcm->index);
 320                 else /* DC_MOTOR_IDLE_MODE */
 321                         DC_MOTOR_IDLE(dcm->index);
 322         }
 323 }
 324
 325 /**
 326  * Apply a confinguration to select DC motor.
 327  */
 328 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
 329 {
 330         DCMotor *dcm = &dcm_all[index];
 331
 332         dcm->cfg = dcm_conf;
 333
 334         /*
 335          * Apply config value.
 336          */
 337         dcm->index = index;
 338
 339         /*
 340          * By default the motor run forever..
 341          */
 342         dcm->expire_time = DC_MOTOR_NO_EXPIRE;
 343
 344         /*
 345          * Clear the status.
 346          */
 347         dcm->status = 0;
 348
 349         // Update registered motors.
 350         dcm_registered_num++;
 351
 352         pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq);
 353         pwm_enable(dcm->cfg->pwm_dev, false);
 354
 355         //Init pid control
 356         pid_control_init(&dcm->pid_ctx, &dcm->cfg->pid_cfg);
 357
 358         /*
 359          * We are using the same sample period for each
 360          * motor, and so we check if this value is the same
 361          * for all. The sample period time is defined in pid
 362          * configuration.
 363          *
 364          * TODO: Use a different sample period for each motor
 365          * and refactor a module to allow to use a timer interrupt,
 366          * in this way we can controll a DC motor also without a
 367          * kernel, increasing a portability on other target.
 368          */
 369         if (!sample_period)
 370                 sample_period = dcm->cfg->pid_cfg.sample_period;
 371
 372         ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period);
 373
 374         //Set default direction for DC motor
 375         DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir);
 376         DC_MOTOR_SET_STATUS_DIR(dcm->status, dcm->cfg->dir);
 377
 378         LOG_INFO("DC motor[%d]:\n", dcm->index);
 379         LOG_INFO("> PID: kp[%f],ki[%f],kd[%f]\n", dcm->cfg->pid_cfg.kp, dcm->cfg->pid_cfg.ki, dcm->cfg->pid_cfg.kd);
 380         LOG_INFO("> PWM: pwm_dev[%d], freq[%ld], sample[%ld]\n", dcm->cfg->pwm_dev, dcm->cfg->freq, dcm->cfg->sample_delay);
 381         LOG_INFO("> ADC: adc_ch[%d], adc_max[%d], adc_min[%d]\n", dcm->cfg->adc_ch, dcm->cfg->adc_max, dcm->cfg->adc_min);
 382         LOG_INFO("> DC: dir[%d], speed[%d]\n", dcm->cfg->dir, dcm->cfg->speed);
 383
 384 }
 385
 386
 387 /**
 388  * Init DC motor.
 389  * \a priority: sets the dc motor process priority.
 390  */
 391 void dc_motor_init(int priority)
 392 {
 393         ASSERT(CONFIG_KERN);
 394
 395         struct Process *dc_motor;
 396
 397         memset(dcm_all, 0, sizeof(dcm_all));
 398
 399         // Init a sample period
 400         sample_period = 0;
 401
 402         // Count how much motor we have to manage.
 403         dcm_registered_num = 0;
 404
 405         MOTOR_DC_INIT();
 406
 407         //Create a dc motor poll process
 408         dc_motor = proc_new_with_name("DC_Motor", dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);
 409         proc_setPri(dc_motor, priority);
 410
 411 }
 412