4 * This file is part of BeRTOS.
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.
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.
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
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.
29 * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
33 * \brief DC motor driver (implementation)
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.
45 * \author Daniele Basile <asterix@develer.com>
49 #include "hw/hw_dc_motor.h"
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
56 #include <cfg/debug.h>
58 #include <algo/pid_control.h>
60 #include <drv/timer.h>
62 #include <kern/proc.h>
67 * Define status bit for DC motor device.
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.
73 * Some utility macro for motor directions
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
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))
86 * DC motor definition.
88 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
89 static int dcm_registered_num;
91 // Stack process for DC motor poll.
92 static PROC_DEFINE_STACK(dc_motor_poll_stack, 400);
94 // Sample period for all DC motor.
95 static mtime_t sample_period;
98 LOG_INFOB(static int debug_msg_delay = 0;);
101 INLINE dc_speed_t dc_motor_readSpeed(int index)
103 DCMotor *dcm = &dcm_all[index];
104 LOG_INFO("DC motor[%d]\n", index);
106 return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max);
110 * Read the target speed from select device.
112 dc_speed_t dc_motor_readTargetSpeed(int index)
114 DCMotor *dcm = &dcm_all[index];
115 LOG_INFO("DC motor[%d]\n", index);
117 return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED);
123 * Sampling a signal on DC motor and compute
124 * a new value of speed according with PID control.
126 static void dc_motor_do(int index)
128 DCMotor *dcm = &dcm_all[index];
130 dc_speed_t curr_pos = 0;
133 //If select DC motor is not active we return
134 if (!(dcm->status & DC_MOTOR_ACTIVE))
138 //Compute next value for reaching target speed from current position
139 if (dcm->cfg->pid_enable)
141 curr_pos = dc_motor_readSpeed(index);
142 new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
145 new_pid = dcm->tgt_speed;
147 LOG_INFOB(if (debug_msg_delay == 20)
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);
156 //Apply the compute duty value
157 pwm_setDuty(dcm->cfg->pwm_dev, new_pid);
160 pwm_enable(dcm->cfg->pwm_dev, true);
162 DC_MOTOR_ENABLE(dcm->index);
167 * Check if the DC motor run time is expired, if this happend
168 * we turn off motor and reset status.
170 INLINE bool check_timerIsExpired(int index)
172 if (((dcm_all[index].expire_time - timer_clock()) < 0) &&
173 (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE))
175 dc_motor_enable(index, false, DC_MOTOR_IDLE_MODE);
176 dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
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
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.
197 static void NORETURN dc_motor_poll(void)
201 mtime_t max_sample_delay = 0;
205 * For all DC motor we read and process output singal,
206 * and choose the max value to off time
208 for (i = 0; i < dcm_registered_num; i++)
210 if (check_timerIsExpired(i))
213 max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay);
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.
221 if (dcm_all[i].cfg->enable_dev_speed)
222 dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i));
225 //Wait for next sampling
226 timer_delay(sample_period - max_sample_delay);
228 for (i = 0; i < dcm_registered_num; i++)
230 check_timerIsExpired(i);
232 if (dcm_all[i].status & DC_MOTOR_ACTIVE)
234 DC_MOTOR_DISABLE(dcm_all[i].index);
235 pwm_enable(dcm_all[i].cfg->pwm_dev, false);
239 //Wait some time to allow signal to stabilize before sampling
240 timer_delay(max_sample_delay);
245 * Set spin direction of DC motor.
247 * \a index number of DC motor
248 * \a dir direction of DC motor
250 void dc_motor_setDir(int index, bool dir)
252 DCMotor *dcm = &dcm_all[index];
255 * To set dc motor direction we must also set the
256 * PWM polarity according with dc motor driver chip
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);
265 * Set DC motor speed.
267 void dc_motor_setSpeed(int index, dc_speed_t speed)
269 DCMotor *dcm = &dcm_all[index];
271 dcm->tgt_speed = speed;
273 LOG_INFO("DC Motor[%d]: Tspeed[%d]\n", index, dcm->tgt_speed);
277 * Set among of time that dc motor should run.
279 void dc_motor_setTimer(int index, mtime_t on_time)
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);
287 * Enable or disable dc motor.
289 * There are two \a mode to disable the dc motor:
290 * - DC_MOTOR_DISABLE_MODE
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.
299 void dc_motor_enable(int index, bool state, int mode)
301 DCMotor *dcm = &dcm_all[index];
304 * Clean all PID stutus variable, becouse
305 * we start with new one.
307 pid_control_reset(&dcm->pid_ctx);
311 dcm->status |= DC_MOTOR_ACTIVE;
315 pwm_enable(dcm->cfg->pwm_dev, false);
316 dcm->status &= ~DC_MOTOR_ACTIVE;
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);
326 * Apply a confinguration to select DC motor.
328 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
330 DCMotor *dcm = &dcm_all[index];
335 * Apply config value.
340 * By default the motor run forever..
342 dcm->expire_time = DC_MOTOR_NO_EXPIRE;
349 // Update registered motors.
350 dcm_registered_num++;
352 pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq);
353 pwm_enable(dcm->cfg->pwm_dev, false);
356 pid_control_init(&dcm->pid_ctx, &dcm->cfg->pid_cfg);
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
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.
370 sample_period = dcm->cfg->pid_cfg.sample_period;
372 ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period);
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);
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);
389 * \a priority: sets the dc motor process priority.
391 void dc_motor_init(int priority)
395 struct Process *dc_motor;
397 memset(dcm_all, 0, sizeof(dcm_all));
399 // Init a sample period
402 // Count how much motor we have to manage.
403 dcm_registered_num = 0;
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);