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