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  *
  38  * TODO: write a brief..
  39  *
  40  * \author Daniele Basile <asterix@develer.com>
  41  */
  42
  43 #include "dc_motor.h"
  44 #include "hw/hw_dc_motor.h"
  45
  46 #include <algo/pid_control.h>
  47
  48 #include <drv/timer.h>
  49 #include <drv/adc.h>
  50
  51 #include <cfg/debug.h>
  52
  53 #include <kern/proc.h>
  54
  55 #define DC_MOTOR_TARGET_TRIM 0
  56
  57 #if DC_MOTOR_TARGET_TRIM
  58         #define DC_MOTOR_TARGET_TRIM_SAMPLE_PER 50
  59         static int tgt_sample_period = 0;
  60 #endif
  61
  62 /**
  63  * DC motor definition.
  64  */
  65 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
  66
  67 /// Stack process for DC motor poll.
  68 static cpustack_t dc_motor_poll_stack[200];
  69
  70 ///< Sample period for all DC motor.
  71 static mtime_t sample_period;
  72
  73 // Only for Debug
  74 // static int debug_msg_delay = 0;
  75
  76 //Forward declaration
  77 static void dc_motor_do(int index);
  78
  79 /**
  80  * Process to poll DC motor status.
  81  * To use a Back-EMF technique (see brief for more details),
  82  * we turn off a motor for max_sample_delay, that value are stored
  83  * in each DC motor config. For this implementation we assume
  84  * that have a common max_sample_delay, choose among a max delay
  85  * to all DC motor configuration.
  86  * The DC motor off time is choose to allow the out signal to
  87  * be stable, so we can read and process this value for feedback
  88  * controll loop.
  89  * The period (sample_period - max_sample_delay) that every time
  90  * we turn off a DC motor is choose to have a feedback controll
  91  * more responsive or less responsive.
  92  */
  93 static void NORETURN dc_motor_poll(void)
  94 {
  95         for (;;)
  96         {
  97                 mtime_t max_sample_delay = 0;
  98                 int i;
  99
 100                 /*
 101                  * For all DC motor we read and process output singal,
 102                  * and choose the max value to off time
 103                  */
 104                 for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 105                 {
 106                         dc_motor_do(i);
 107                         max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay);
 108                 }
 109
 110                 //Wait for next sampling
 111                 timer_delay(sample_period - max_sample_delay);
 112
 113                 for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 114                 {
 115                         if (dcm_all[i].status & DC_MOTOR_ACTIVE)
 116                         {
 117                                 DC_MOTOR_DISABLE(dcm_all[i].index);
 118                                 pwm_enable(dcm_all[i].cfg->pwm_dev, false);
 119                         }
 120                 }
 121
 122                 //Wait some time to allow signal to stabilize before sampling
 123                 timer_delay(max_sample_delay);
 124
 125 #if DC_MOTOR_TARGET_TRIM
 126                 if (tgt_sample_period == DC_MOTOR_TARGET_TRIM_SAMPLE_PER)
 127                 {
 128                         dc_speed_t tgt_speed = ADC_RANGECONV(adc_read(6), 0, 65535);
 129
 130                         for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 131                         {
 132                                 if ((dcm_all[i].tgt_speed >= tgt_speed - 100) && (dcm_all[i].tgt_speed <= tgt_speed + 100))
 133                                         TRACEMSG("DCmt[%d]> tgt[%d]", i, tgt_speed);
 134
 135                                 dc_motor_setSpeed(i, tgt_speed);
 136                         }
 137                         tgt_sample_period = 0;
 138                 }
 139                 tgt_sample_period++;
 140 #endif
 141         }
 142 }
 143
 144 /*
 145  * Sampling a signal on DC motor and compute
 146  * a new value of speed according with PID control.
 147  */
 148 static void dc_motor_do(int index)
 149 {
 150         DCMotor *dcm = &dcm_all[index];
 151
 152         dc_speed_t curr_pos;
 153         pwm_duty_t new_pid;
 154
 155         //If select DC motor is not active we return
 156         if (!(dcm->status & DC_MOTOR_ACTIVE))
 157                 return;
 158
 159         //Acquire the output signal
 160         curr_pos = ADC_RANGECONV(adc_read(dcm->cfg->adc_ch), dcm->cfg->adc_min, dcm->cfg->adc_max);
 161
 162         //Compute next value for reaching target speed from current position
 163         new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
 164
 165 //TODO: Add a multilevel debug..
 166 //      if (debug_msg_delay == 20)
 167 //      {
 168 //              TRACEMSG("DCmt[%d]>curr_speed[%d],tgt[%d],err[%f],P[%f],I[%f],D[%f]", dcm->index, curr_pos, dcm->tgt_speed);
 169 //              kprintf("%d,", curr_pos);
 170 //              debug_msg_delay = 0;
 171 //      }
 172 //      debug_msg_delay++;
 173
 174 //      TRACEMSG("tg[%d], new_pid[%d], pos[%d]", dcm->tgt_speed, new_pid, curr_pos);
 175
 176         //Apply the compute duty value
 177         pwm_setDuty(dcm->cfg->pwm_dev, new_pid);
 178
 179         //Restart dc motor
 180         pwm_enable(dcm->cfg->pwm_dev, true);
 181
 182         DC_MOTOR_ENABLE(dcm->index);
 183 }
 184
 185 /**
 186  * Set spin direction of DC motor.
 187  *
 188  * \a index number of DC motor
 189  * \a dir direction of DC motor
 190  */
 191 void dc_motor_setDir(int index, bool dir)
 192 {
 193         DCMotor *dcm = &dcm_all[index];
 194
 195         if (dir != (dcm->status & DC_MOTOR_DIR))
 196         {
 197                 //Reset previous direction flag, and set new
 198                 dcm->status &= ~DC_MOTOR_DIR;
 199                 dcm->status |= DC_MOTOR_DIR;
 200                 DC_MOTOR_SET_DIR(index, dir);
 201         }
 202
 203 }
 204
 205
 206 /**
 207  * Set DC motor speed.
 208  */
 209 void dc_motor_setSpeed(int index, dc_speed_t speed)
 210 {
 211         dcm_all[index].tgt_speed = speed;
 212 }
 213
 214 /**
 215  * Enable or disable dc motor
 216  */
 217 void dc_motor_enable(int index, bool state)
 218 {
 219         DCMotor *dcm = &dcm_all[index];
 220
 221         /*
 222          * Clean all PID stutus variable, becouse
 223          * we start with new one.
 224          */
 225         pid_control_reset(&dcm->pid_ctx);
 226
 227         if (state)
 228         {
 229                 dcm->status |= DC_MOTOR_ACTIVE;
 230         }
 231         else
 232         {
 233                 pwm_enable(dcm->cfg->pwm_dev, false);
 234                 DC_MOTOR_DISABLE(dcm->index);
 235
 236                 dcm->status &= ~DC_MOTOR_ACTIVE;
 237         }
 238 }
 239
 240 /**
 241  * Apply a confinguration to select DC motor.
 242  */
 243 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
 244 {
 245         DCMotor *dcm = &dcm_all[index];
 246
 247         dcm->cfg = dcm_conf;
 248
 249         /*
 250          * Apply config value.
 251          */
 252         dcm->index = index;
 253
 254         DC_MOTOR_INIT(dcm->index);
 255
 256         pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq);
 257         pwm_enable(dcm->cfg->pwm_dev, false);
 258
 259         //Init pid control
 260         pid_control_init(&dcm->pid_ctx, &dcm->cfg->pid_cfg);
 261
 262         /*
 263          * We are using the same sample period for each
 264          * motor, and so we check if this value is the same
 265          * for all. The sample period time is defined in pid
 266          * configuration.
 267          *
 268          * TODO: Use a different sample period for each motor
 269          * and refactor a module to allow to use a timer interrupt,
 270          * in this way we can controll a DC motor also without a
 271          * kernel, increasing a portability on other target.
 272          */
 273         if (!sample_period)
 274                 sample_period = dcm->cfg->pid_cfg.sample_period;
 275
 276         ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period);
 277
 278         //Set default direction for DC motor
 279         DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir);
 280
 281         TRACEMSG("DC motort[%d]> kp[%f],ki[%f],kd[%f]", dcm->index, dcm_conf->pid_cfg.kp, dcm_conf->pid_cfg.ki, dcm_conf->pid_cfg.kd);
 282         TRACEMSG("               > pwm_dev[%d], freq[%ld], sample[%ld]", dcm_conf->pwm_dev, dcm_conf->freq, dcm_conf->sample_delay);
 283         TRACEMSG("               > adc_ch[%d], adc_max[%d], adc_min[%d]", dcm_conf->adc_ch, dcm_conf->adc_max, dcm_conf->adc_min);
 284         TRACEMSG("               > dir[%d]", dcm->cfg->dir);
 285
 286 }
 287
 288
 289 /**
 290  * Init DC motor
 291  */
 292 void dc_motor_init(void)
 293 {
 294         for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
 295                 dcm_all[i].status &= ~DC_MOTOR_ACTIVE;
 296
 297         //Init a sample period
 298         sample_period = 0;
 299
 300         //Create a dc motor poll process
 301         proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);
 302
 303 }
 304