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 based, on the capability of the DC motor to become a generator
  38  * of voltage when we turn off its supply. This happen every time we turn off the
  39  * DC motor supply, and it continues to rotate for a short time thanks to its mechanical
  40  * energy. Using this idea we can turn off the motor for a very short time, and
  41  * we read the volage value from DC motor supply pins. This voltage say to us
  42  * the actual speed of the motor.
  43  *
  44  * \author Daniele Basile <asterix@develer.com>
  45  */
  46
  47 #include "dc_motor.h"
  48 #include "hw/hw_dc_motor.h"
  49
  50 // Define logging setting (for cfg/log.h module).
  51 #define LOG_LEVEL         DC_MOTOR_LOG_LEVEL
  52 #define LOG_FORMAT        DC_MOTOR_LOG_FORMAT
  53
  54 #include <cfg/log.h>
  55 #include <cfg/debug.h>
  56
  57 #include <algo/pid_control.h>
  58
  59 #include <drv/timer.h>
  60
  61 #include <kern/proc.h>
  62
  63 #include <cpu/power.h>
  64
  65 #include <string.h>
  66
  67 /**
  68  * Define status bit for DC motor device.
  69  */
  70 #define DC_MOTOR_ACTIVE           BV(0)     ///< DC motor enable or disable flag.
  71 #define DC_MOTOR_DIR              BV(1)     ///< Spin direction of DC motor.
  72
  73 /*
  74  * Some utility macro for motor directions
  75  */
  76 #define POS_DIR                   1
  77 #define NEG_DIR                   0
  78 #define DC_MOTOR_POS_DIR(x)       ((x) |= DC_MOTOR_DIR)   // Set directions status positive
  79 #define DC_MOTOR_NEG_DIR(x)       ((x) &= ~DC_MOTOR_DIR)  // Set directions status negative
  80
  81 // Update the status with current direction
  82 #define DC_MOTOR_SET_STATUS_DIR(status, dir) \
  83                 (dir == POS_DIR ? DC_MOTOR_POS_DIR(status) : DC_MOTOR_NEG_DIR(status))
  84
  85 #if CONFIG_KERN_PREEMPT
  86         #if CONFIG_DC_MOTOR_USE_SEM
  87                 #include <kern/sem.h>
  88
  89                 Semaphore dc_motor_sem;
  90                 #define DC_MOTOR_LOCK        sem_obtain(&dc_motor_sem)
  91                 #define DC_MOTOR_UNLOCK      sem_release(&dc_motor_sem)
  92         #else
  93                 #define DC_MOTOR_LOCK        proc_forbid()
  94                 #define DC_MOTOR_UNLOCK      proc_permit()
  95         #endif
  96 #else
  97         #define DC_MOTOR_LOCK        /* None */
  98         #define DC_MOTOR_UNLOCK      /* None */
  99 #endif
 100
 101 /**
 102  * DC motor definition.
 103  */
 104 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
 105
 106 /*
 107  * Process to poll dc motor status
 108  */
 109 struct Process *dc_motor;
 110
 111
 112 // Stack process for DC motor poll.
 113 static PROC_DEFINE_STACK(dc_motor_poll_stack, 500);
 114
 115 // Only for Debug
 116 LOG_INFOB(static int debug_msg_delay = 0;);
 117
 118
 119 INLINE dc_speed_t dc_motor_readSpeed(int index)
 120 {
 121         DCMotor *dcm = &dcm_all[index];
 122         return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max);
 123 }
 124
 125 /**
 126  * Read the target speed from select device.
 127  */
 128 dc_speed_t dc_motor_readTargetSpeed(int index)
 129 {
 130         DCMotor *dcm = &dcm_all[index];
 131         return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED);
 132 }
 133
 134 static void dc_motor_start(int index)
 135 {
 136         DCMotor *dcm = &dcm_all[index];
 137
 138         DC_MOTOR_LOCK;
 139         /*
 140          * Clean all PID stutus variable, becouse
 141          * we start with new one.
 142          */
 143         pid_control_reset(&dcm->pid_ctx);
 144         dcm->status |= DC_MOTOR_ACTIVE;
 145         DC_MOTOR_UNLOCK;
 146 }
 147
 148 /*
 149  * There are two \a mode to stop the dc motor:
 150  *  - DC_MOTOR_DISABLE_MODE
 151  *  - DC_MOTOR_IDLE
 152  *
 153  * The DC_MOTOR_DISABLE_MODE shut down the DC motor and
 154  * leave it floating to rotate.
 155  * The DC_MOTOR_IDLE does not shut down DC motor, but put
 156  * its supply pin in short circuite, in this way the motor result
 157  * braked from intentional rotation.
 158  */
 159 static void dc_motor_stop(int index)
 160 {
 161         DCMotor *dcm = &dcm_all[index];
 162
 163         DC_MOTOR_LOCK;
 164
 165         dcm->status &= ~DC_MOTOR_ACTIVE;
 166         dcm->expire_time = DC_MOTOR_NO_EXPIRE;
 167         pwm_enable(dcm->cfg->pwm_dev, false);
 168
 169         if (dcm->cfg->braked)
 170         {
 171                 DC_MOTOR_STOP_BRAKED(dcm->index);
 172         }
 173         else
 174         {
 175                 DC_MOTOR_STOP_FLOAT(dcm->index);
 176         }
 177
 178         DC_MOTOR_UNLOCK;
 179 }
 180
 181 /*
 182  * Sampling a signal on DC motor and compute
 183  * a new value of speed according with PID control.
 184  */
 185 static void dc_motor_do(int index)
 186 {
 187         DCMotor *dcm = &dcm_all[index];
 188
 189         dc_speed_t curr_pos = 0;
 190         pwm_duty_t new_pid = 0;
 191
 192         DC_MOTOR_LOCK;
 193
 194         //If select DC motor is not active we return
 195         if (!dcm->status & DC_MOTOR_ACTIVE)
 196         {
 197                 DC_MOTOR_UNLOCK;
 198                 return;
 199         }
 200
 201         /*
 202          * To set dc motor direction we must also set the
 203          * PWM polarity according with dc motor driver chip
 204          */
 205         pwm_setPolarity(dcm->cfg->pwm_dev, dcm->status & DC_MOTOR_DIR);
 206         DC_MOTOR_SET_DIR(dcm->index, dcm->status & DC_MOTOR_DIR);
 207
 208         //Compute next value for reaching target speed from current position
 209         if (dcm->cfg->pid_enable)
 210         {
 211                 /*
 212                  * Here we cannot disable the switch context because the
 213                  * driver, that read the speed could be need to use signal or
 214                  * other thing that needs the kernel switch context, for this
 215                  * reason we unlock before to read the speed.
 216                  */
 217                 DC_MOTOR_UNLOCK;
 218                 curr_pos = dc_motor_readSpeed(index);
 219                 DC_MOTOR_LOCK;
 220                 new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
 221         }
 222         else
 223         {
 224                 new_pid = dcm->tgt_speed;
 225         }
 226
 227         LOG_INFOB(
 228                 if (debug_msg_delay == 20)
 229                 {
 230                         LOG_INFO("DC Motor[%d]: curr_speed[%d],curr_pos[%d],tgt[%d]\n", dcm->index,
 231                                                                 curr_pos, new_pid, dcm->tgt_speed);
 232                         debug_msg_delay = 0;
 233                 }
 234                 debug_msg_delay++;
 235         );
 236
 237         //Apply the compute duty value
 238         pwm_setDuty(dcm->cfg->pwm_dev, new_pid);
 239
 240         //Restart dc motor
 241         pwm_enable(dcm->cfg->pwm_dev, true);
 242
 243         DC_MOTOR_ENABLE(dcm->index);
 244         DC_MOTOR_UNLOCK;
 245 }
 246
 247
 248 /*
 249  * Check if the DC motor run time is expired, if this happend
 250  * we turn off motor and reset status.
 251  */
 252 INLINE bool check_timerIsExpired(int index)
 253 {
 254
 255         DC_MOTOR_LOCK;
 256         bool check = ((dcm_all[index].expire_time - timer_clock()) < 0) &&
 257                         (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE);
 258         DC_MOTOR_UNLOCK;
 259
 260         return check;
 261 }
 262
 263 /**
 264  * Process to poll DC motor status.
 265  * To use a Back-EMF technique (see brief for more details),
 266  * we turn off a motor for CONFIG_DC_MOTOR_SAMPLE_DELAY, that value are stored
 267  * in each DC motor config. For this implementation we assume
 268  * that have a common CONFIG_DC_MOTOR_SAMPLE_DELAY, choose among a max delay
 269  * to all DC motor configuration.
 270  * The DC motor off time is choose to allow the out signal to
 271  * be stable, so we can read and process this value for feedback controll loop.
 272  * The period (CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY)
 273  * that every time we turn off a DC motor is choose to have a feedback controll
 274  * more responsive or less responsive.
 275  */
 276 static void NORETURN dc_motor_poll(void)
 277 {
 278         for (;;)
 279         {
 280                 /*
 281                  * For all DC motor we read and process output singal,
 282                  * and choose the max value to off time
 283                  */
 284                 for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 285                 {
 286                         if (!dcm_all[i].cfg)
 287                                 continue;
 288
 289                         if (check_timerIsExpired(i))
 290                                 dc_motor_stop(i);
 291                         else
 292                                 dc_motor_do(i);
 293
 294                         /*
 295                          * If we read speed from trimmer we update the target
 296                          * speed value when motor is running so we can make
 297                          * dc motor speed regulation.
 298                          */
 299                         if (dcm_all[i].cfg->speed_dev_id != DC_MOTOR_NO_DEV_SPEED)
 300                                 dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i));
 301                 }
 302
 303                 //Wait for next sampling
 304                 timer_delay(CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY);
 305
 306                 for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 307                 {
 308                         if (!dcm_all[i].cfg)
 309                                 continue;
 310
 311                         if (check_timerIsExpired(i))
 312                                 dc_motor_stop(i);
 313
 314                         DC_MOTOR_LOCK;
 315                         if (dcm_all[i].status & DC_MOTOR_ACTIVE)
 316                         {
 317                                 DC_MOTOR_DISABLE(dcm_all[i].index);
 318                                 pwm_enable(dcm_all[i].cfg->pwm_dev, false);
 319                         }
 320                         DC_MOTOR_UNLOCK;
 321                 }
 322
 323                 //Wait some time to allow signal to stabilize before sampling
 324                 timer_delay(CONFIG_DC_MOTOR_SAMPLE_DELAY);
 325         }
 326 }
 327
 328 /**
 329  * Set spin direction of DC motor.
 330  *
 331  * \a index number of DC motor
 332  * \a dir direction of DC motor
 333  */
 334 void dc_motor_setDir(int index, bool dir)
 335 {
 336         DCMotor *dcm = &dcm_all[index];
 337         DC_MOTOR_LOCK;
 338         DC_MOTOR_SET_STATUS_DIR(dcm->status, dir);
 339         DC_MOTOR_UNLOCK;
 340 }
 341
 342 /**
 343  * Set DC motor speed.
 344  */
 345 void dc_motor_setSpeed(int index, dc_speed_t speed)
 346 {
 347         DCMotor *dcm = &dcm_all[index];
 348
 349         DC_MOTOR_LOCK;
 350         dcm->tgt_speed = speed;
 351         DC_MOTOR_UNLOCK;
 352
 353         LOG_INFO("DC Motor[%d]: tgt_speed[%d]\n", index, dcm->tgt_speed);
 354 }
 355
 356 /**
 357  * Set among of time that dc motor should run.
 358  */
 359 void dc_motor_startTimer(int index, mtime_t on_time)
 360 {
 361         DC_MOTOR_LOCK;
 362         dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
 363         if (on_time != DC_MOTOR_NO_EXPIRE)
 364         {
 365                 dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time);
 366                 dc_motor_start(index);
 367         }
 368         DC_MOTOR_UNLOCK;
 369 }
 370
 371 void dc_motor_waitStop(int index)
 372 {
 373         DCMotor *dcm = &dcm_all[index];
 374         bool loop = true;
 375
 376         while (loop)
 377         {
 378                 DC_MOTOR_LOCK;
 379                 loop = dcm->status & DC_MOTOR_ACTIVE;
 380                 DC_MOTOR_UNLOCK;
 381
 382                 cpu_relax();
 383         }
 384 }
 385
 386 /**
 387  * Enable or disable dc motor.
 388  */
 389 void dc_motor_enable(int index, bool state)
 390 {
 391         if (state)
 392                 dc_motor_start(index);
 393         else
 394                 dc_motor_stop(index);
 395 }
 396
 397 /**
 398  * Apply a confinguration to select DC motor.
 399  */
 400 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
 401 {
 402         DCMotor *dcm = &dcm_all[index];
 403
 404         DC_MOTOR_LOCK;
 405         /*
 406          * We are using the same sample period for each
 407          * motor, and so we check if this value is the same
 408          * for all. The sample period time is defined in pid
 409          * configuration.
 410          *
 411          * TODO: Use a different sample period for each motor
 412          * and refactor a module to allow to use a timer interrupt,
 413          * in this way we can controll a DC motor also without a
 414          * kernel, increasing a portability on other target.
 415          */
 416         pid_control_setPeriod(&dcm_conf->pid_cfg, CONFIG_DC_MOTOR_SAMPLE_PERIOD);
 417
 418         //Init pid control
 419         pid_control_init(&dcm->pid_ctx, &dcm_conf->pid_cfg);
 420
 421
 422         dcm->cfg = dcm_conf;
 423
 424         /*
 425          * Apply config value.
 426          */
 427         dcm->index = index;
 428
 429         /*
 430          * By default the motor run forever..
 431          */
 432         dcm->expire_time = DC_MOTOR_NO_EXPIRE;
 433
 434         /*
 435          * By default set target speed.
 436          */
 437         dcm->tgt_speed = dcm_conf->speed;
 438
 439         /*
 440          * Clear the status.
 441          */
 442         dcm->status = 0;
 443
 444         pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq);
 445         pwm_enable(dcm->cfg->pwm_dev, false);
 446
 447         //Set default direction for DC motor
 448         DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir);
 449         DC_MOTOR_SET_STATUS_DIR(dcm->status, dcm->cfg->dir);
 450
 451         DC_MOTOR_UNLOCK;
 452
 453         LOG_INFO("DC motor[%d]:\n", dcm->index);
 454         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);
 455         LOG_INFO("> PWM: pwm_dev[%d], freq[%ld], sample[%d]\n", dcm->cfg->pwm_dev, dcm->cfg->freq,CONFIG_DC_MOTOR_SAMPLE_DELAY);
 456         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);
 457         LOG_INFO("> DC: dir[%d], speed[%d]\n", dcm->cfg->dir, dcm->cfg->speed);
 458 }
 459
 460 /**
 461  * If we had enabled the priority scheduling, we can adjust the
 462  * DC motor poll process priority.
 463  */
 464 void dc_motor_setPriority(int priority)
 465 {
 466         ASSERT(CONFIG_KERN);
 467         ASSERT(dc_motor);
 468         proc_setPri(dc_motor, priority);
 469 }
 470
 471 /**
 472  * Init DC motor.
 473  * \a priority: sets the dc motor process priority.
 474  */
 475 void dc_motor_init(void)
 476 {
 477         ASSERT(CONFIG_KERN);
 478
 479         MOTOR_DC_INIT();
 480
 481         #if (CONFIG_KERN_PREEMPT && CONFIG_DC_MOTOR_USE_SEM)
 482                 sem_init(&dc_motor_sem);
 483         #endif
 484
 485         //Create a dc motor poll process
 486         dc_motor = proc_new_with_name("DC_Motor", dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);
 487 }
 488