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 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.
44 * \author Daniele Basile <asterix@develer.com>
48 #include "hw/hw_dc_motor.h"
49 #include "cfg/cfg_pwm.h"
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
56 #include <cfg/debug.h>
58 #include <algo/pid_control.h>
60 #include <drv/timer.h>
62 #include <kern/proc.h>
64 #include <cpu/power.h>
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))
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))
81 * Define status bit for DC motor device.
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.
87 * Some utility macro for motor directions
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
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))
98 #if (CONFIG_KERN && CONFIG_KERN_PREEMPT)
99 #if CONFIG_DC_MOTOR_USE_SEM
100 #include <kern/sem.h>
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)
106 #define DC_MOTOR_LOCK proc_forbid()
107 #define DC_MOTOR_UNLOCK proc_permit()
110 #define DC_MOTOR_LOCK /* None */
111 #define DC_MOTOR_UNLOCK /* None */
115 * DC motor definition.
117 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
120 * Process to poll dc motor status
122 struct Process *dc_motor;
125 // Stack process for DC motor poll.
126 static PROC_DEFINE_STACK(dc_motor_poll_stack, 500);
129 LOG_INFOB(static int debug_msg_delay = 0;);
132 INLINE dc_speed_t dc_motor_readSpeed(int index)
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);
139 * Read the target speed from select device.
141 dc_speed_t dc_motor_readTargetSpeed(int index)
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);
147 static void dc_motor_start(int index)
149 DCMotor *dcm = &dcm_all[index];
153 * Clean all PID stutus variable, becouse
154 * we start with new one.
156 pid_control_reset(&dcm->pid_ctx);
157 dcm->status |= DC_MOTOR_ACTIVE;
162 * There are two \a mode to stop the dc motor:
163 * - DC_MOTOR_DISABLE_MODE
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.
172 static void dc_motor_stop(int index)
174 DCMotor *dcm = &dcm_all[index];
178 dcm->status &= ~DC_MOTOR_ACTIVE;
179 dcm->expire_time = DC_MOTOR_NO_EXPIRE;
180 PWM_ENABLE(dcm, false);
182 if (dcm->cfg->braked)
184 DC_MOTOR_STOP_BRAKED(dcm->index);
188 DC_MOTOR_STOP_FLOAT(dcm->index);
195 * Sampling a signal on DC motor and compute
196 * a new value of speed according with PID control.
198 static void dc_motor_do(int index)
200 DCMotor *dcm = &dcm_all[index];
202 dc_speed_t curr_pos = 0;
203 pwm_duty_t new_pid = 0;
207 //If select DC motor is not active we return
208 if (!(dcm->status & DC_MOTOR_ACTIVE))
215 * To set dc motor direction we must also set the
216 * PWM polarity according with dc motor driver chip
218 PWM_SETPOL(dcm, dcm->status & DC_MOTOR_DIR);
219 DC_MOTOR_SET_DIR(dcm->index, dcm->status & DC_MOTOR_DIR);
221 //Compute next value for reaching target speed from current position
222 if (dcm->cfg->pid_enable)
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.
231 curr_pos = dc_motor_readSpeed(index);
233 new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
237 new_pid = dcm->tgt_speed;
241 if (debug_msg_delay == 20)
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);
250 //Apply the compute duty value
251 PWM_SETDUTY(dcm, new_pid);
254 PWM_ENABLE(dcm, true);
256 DC_MOTOR_ENABLE(dcm->index);
262 * Check if the DC motor run time is expired, if this happend
263 * we turn off motor and reset status.
265 INLINE bool check_timerIsExpired(int index)
269 bool check = ((dcm_all[index].expire_time - timer_clock()) < 0) &&
270 (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE);
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.
289 static void NORETURN dc_motor_poll(void)
294 * For all DC motor we read and process output singal,
295 * and choose the max value to off time
297 for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
302 if (check_timerIsExpired(i))
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.
312 if (dcm_all[i].cfg->speed_dev_id != DC_MOTOR_NO_DEV_SPEED)
313 dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i));
316 //Wait for next sampling
317 timer_delay(CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY);
319 for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
324 if (check_timerIsExpired(i))
328 if (dcm_all[i].status & DC_MOTOR_ACTIVE)
330 DC_MOTOR_DISABLE(dcm_all[i].index);
331 PWM_ENABLE(&dcm_all[i], false);
336 //Wait some time to allow signal to stabilize before sampling
337 timer_delay(CONFIG_DC_MOTOR_SAMPLE_DELAY);
342 * Set spin direction of DC motor.
344 * \a index number of DC motor
345 * \a dir direction of DC motor
347 void dc_motor_setDir(int index, bool dir)
349 DCMotor *dcm = &dcm_all[index];
351 DC_MOTOR_SET_STATUS_DIR(dcm->status, dir);
356 * Set DC motor speed.
358 void dc_motor_setSpeed(int index, dc_speed_t speed)
360 DCMotor *dcm = &dcm_all[index];
363 dcm->tgt_speed = speed;
366 LOG_INFO("DC Motor[%d]: tgt_speed[%d]\n", index, dcm->tgt_speed);
370 * Set among of time that dc motor should run.
372 void dc_motor_startTimer(int index, mtime_t on_time)
375 dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
376 if (on_time != DC_MOTOR_NO_EXPIRE)
378 dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time);
379 dc_motor_start(index);
384 void dc_motor_waitStop(int index)
386 DCMotor *dcm = &dcm_all[index];
392 loop = dcm->status & DC_MOTOR_ACTIVE;
400 * Enable or disable dc motor.
402 void dc_motor_enable(int index, bool state)
405 dc_motor_start(index);
407 dc_motor_stop(index);
411 * Apply a confinguration to select DC motor.
413 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
415 DCMotor *dcm = &dcm_all[index];
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
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.
429 pid_control_setPeriod(&dcm_conf->pid_cfg, CONFIG_DC_MOTOR_SAMPLE_PERIOD);
432 pid_control_init(&dcm->pid_ctx, &dcm_conf->pid_cfg);
438 * Apply config value.
443 * By default the motor run forever..
445 dcm->expire_time = DC_MOTOR_NO_EXPIRE;
448 * By default set target speed.
450 dcm->tgt_speed = dcm_conf->speed;
456 #if !CFG_PWM_ENABLE_OLD_API
457 pwm_init(&dcm->pwm, dcm_conf->pwm_dev);
459 PWM_SETFREQ(dcm, dcm->cfg->freq);
460 PWM_ENABLE(dcm, false);
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);
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);
476 * If we had enabled the priority scheduling, we can adjust the
477 * DC motor poll process priority.
479 void dc_motor_setPriority(int priority)
483 proc_setPri(dc_motor, priority);
488 * \a priority: sets the dc motor process priority.
490 void dc_motor_init(void)
496 #if (CONFIG_KERN_PREEMPT && CONFIG_DC_MOTOR_USE_SEM)
497 sem_init(&dc_motor_sem);
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);