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.
38 * TODO: write a brief..
40 * \author Daniele Basile <asterix@develer.com>
44 #include "hw/hw_dc_motor.h"
46 // Define logging setting (for cfg/log.h module).
47 #define LOG_LEVEL DC_MOTOR_LOG_LEVEL
48 #define LOG_VERBOSITY DC_MOTOR_LOG_FORMAT
51 #include <cfg/debug.h>
53 #include <algo/pid_control.h>
55 #include <drv/timer.h>
58 #include <kern/proc.h>
63 * DC motor definition.
65 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
67 /// Stack process for DC motor poll.
68 static cpu_stack_t dc_motor_poll_stack[1024];
70 ///< Sample period for all DC motor.
71 static mtime_t sample_period;
74 LOG_INFOB(static int debug_msg_delay = 0;);
77 static void dc_motor_do(int index);
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
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.
93 static void NORETURN dc_motor_poll(void)
97 mtime_t max_sample_delay = 0;
101 * For all DC motor we read and process output singal,
102 * and choose the max value to off time
104 for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
107 max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay);
110 //Wait for next sampling
111 timer_delay(sample_period - max_sample_delay);
113 for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
115 if (dcm_all[i].status & DC_MOTOR_ACTIVE)
117 DC_MOTOR_DISABLE(dcm_all[i].index);
118 pwm_enable(dcm_all[i].cfg->pwm_dev, false);
122 //Wait some time to allow signal to stabilize before sampling
123 timer_delay(max_sample_delay);
128 * Sampling a signal on DC motor and compute
129 * a new value of speed according with PID control.
131 static void dc_motor_do(int index)
133 DCMotor *dcm = &dcm_all[index];
138 //If select DC motor is not active we return
139 if (!(dcm->status & DC_MOTOR_ACTIVE))
142 //Acquire the output signal
143 curr_pos = ADC_RANGECONV(adc_read(dcm->cfg->adc_ch), dcm->cfg->adc_min, dcm->cfg->adc_max);
145 //Compute next value for reaching target speed from current position
146 new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
149 LOG_INFOB(if (debug_msg_delay == 20)
151 LOG_INFO("DCmt[%d]>curr_speed[%d],tgt[%d]\n", dcm->index, curr_pos, dcm->tgt_speed);
152 LOG_INFO("%d,", curr_pos);
158 LOG_WARN("tg[%d], new_pid[%d], pos[%d]\n", dcm->tgt_speed, new_pid, curr_pos);
160 //Apply the compute duty value
161 pwm_setDuty(dcm->cfg->pwm_dev, new_pid);
164 pwm_enable(dcm->cfg->pwm_dev, true);
166 DC_MOTOR_ENABLE(dcm->index);
170 * Set spin direction of DC motor.
172 * \a index number of DC motor
173 * \a dir direction of DC motor
175 void dc_motor_setDir(int index, bool dir)
177 DCMotor *dcm = &dcm_all[index];
179 if (dir != (dcm->status & DC_MOTOR_DIR))
181 //Reset previous direction flag, and set new
182 dcm->status &= ~DC_MOTOR_DIR;
183 dcm->status |= DC_MOTOR_DIR;
186 * To set dc motor direction we must also set the
187 * PWM polarity according with dc motor driver chip
189 pwm_setPolarity(dcm->cfg->pwm_dev, dir);
190 DC_MOTOR_SET_DIR(index, dir);
196 * Set DC motor speed.
198 void dc_motor_setSpeed(int index, dc_speed_t speed)
200 dcm_all[index].tgt_speed = speed;
204 * Enable or disable dc motor
206 void dc_motor_enable(int index, bool state)
208 DCMotor *dcm = &dcm_all[index];
211 * Clean all PID stutus variable, becouse
212 * we start with new one.
214 pid_control_reset(&dcm->pid_ctx);
218 dcm->status |= DC_MOTOR_ACTIVE;
222 pwm_enable(dcm->cfg->pwm_dev, false);
223 DC_MOTOR_DISABLE(dcm->index);
225 dcm->status &= ~DC_MOTOR_ACTIVE;
230 * Apply a confinguration to select DC motor.
232 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
234 DCMotor *dcm = &dcm_all[index];
239 * Apply config value.
243 pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq);
244 pwm_setPolarity(dcm->cfg->pwm_dev, dcm->cfg->pol);
245 pwm_enable(dcm->cfg->pwm_dev, false);
248 pid_control_init(&dcm->pid_ctx, &dcm->cfg->pid_cfg);
251 * We are using the same sample period for each
252 * motor, and so we check if this value is the same
253 * for all. The sample period time is defined in pid
256 * TODO: Use a different sample period for each motor
257 * and refactor a module to allow to use a timer interrupt,
258 * in this way we can controll a DC motor also without a
259 * kernel, increasing a portability on other target.
262 sample_period = dcm->cfg->pid_cfg.sample_period;
264 ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period);
266 //Set default direction for DC motor
267 DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir);
269 LOG_INFO("DC motort[%d]:\n", dcm->index);
270 LOG_INFO("> PID: kp[%lf],ki[%lf],kd[%lf]\n", dcm->cfg->pid_cfg.kd, dcm->cfg->pid_cfg.ki, dcm->cfg->pid_cfg.kd);
271 LOG_INFO("> PWM: pwm_dev[%d], freq[%ld], sample[%ld]\n", dcm->cfg->pwm_dev, dcm->cfg->freq, dcm->cfg->sample_delay);
272 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);
273 LOG_INFO("> DC: dir[%d], speed[%d]\n", dcm->cfg->dir, dcm->cfg->speed);
281 void dc_motor_init(void)
283 memset(dcm_all, 0, sizeof(dcm_all));
285 //Init a sample period
290 //Create a dc motor poll process
291 proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);