From: asterix Date: Fri, 2 May 2008 11:51:48 +0000 (+0000) Subject: DC motor driver module. X-Git-Tag: 1.0.0~8 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=674d5052e41cd7fc5ce3d05ac44e7b543cd34016;p=bertos.git DC motor driver module. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1244 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c new file mode 100644 index 00000000..1df0083e --- /dev/null +++ b/bertos/drv/dc_motor.c @@ -0,0 +1,304 @@ +/** + * \file + * + * + * + * \brief DC motor driver (implementation) + * + * Thi module provide a simple api to controll a DC motor in direction and + * speed, to allow this we use a Back-EMF technique. + * + * TODO: write a brief.. + * + * \author Daniele Basile + */ + +#include "dc_motor.h" +#include "hw_dc_motor.h" + +#include + +#include +#include + +#include + +#include + +#define DC_MOTOR_TARGET_TRIM 0 + +#if DC_MOTOR_TARGET_TRIM + #define DC_MOTOR_TARGET_TRIM_SAMPLE_PER 50 + static int tgt_sample_period = 0; +#endif + +/** + * DC motor definition. + */ +static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR]; + +/// Stack process for DC motor poll. +static cpustack_t dc_motor_poll_stack[200]; + +///< Sample period for all DC motor. +static mtime_t sample_period; + +// Only for Debug +// static int debug_msg_delay = 0; + +//Forward declaration +static void dc_motor_do(int index); + +/** + * Process to poll DC motor status. + * To use a Back-EMF technique (see brief for more details), + * we turn off a motor for max_sample_delay, that value are stored + * in each DC motor config. For this implementation we assume + * that have a common max_sample_delay, choose among a max delay + * to all DC motor configuration. + * The DC motor off time is choose to allow the out signal to + * be stable, so we can read and process this value for feedback + * controll loop. + * The period (sample_period - max_sample_delay) that every time + * we turn off a DC motor is choose to have a feedback controll + * more responsive or less responsive. + */ +static void NORETURN dc_motor_poll(void) +{ + for (;;) + { + mtime_t max_sample_delay = 0; + int i; + + /* + * For all DC motor we read and process output singal, + * and choose the max value to off time + */ + for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + { + dc_motor_do(i); + max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay); + } + + //Wait for next sampling + timer_delay(sample_period - max_sample_delay); + + for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + { + if (dcm_all[i].status & DC_MOTOR_ACTIVE) + { + DC_MOTOR_DISABLE(dcm_all[i].index); + pwm_enable(dcm_all[i].cfg->pwm_dev, false); + } + } + + //Wait some time to allow signal to stabilize before sampling + timer_delay(max_sample_delay); + +#if DC_MOTOR_TARGET_TRIM + if (tgt_sample_period == DC_MOTOR_TARGET_TRIM_SAMPLE_PER) + { + dc_speed_t tgt_speed = ADC_RANGECONV(adc_read(6), 0, 65535); + + for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + { + if ((dcm_all[i].tgt_speed >= tgt_speed - 100) && (dcm_all[i].tgt_speed <= tgt_speed + 100)) + TRACEMSG("DCmt[%d]> tgt[%d]", i, tgt_speed); + + dc_motor_setSpeed(i, tgt_speed); + } + tgt_sample_period = 0; + } + tgt_sample_period++; +#endif + } +} + +/* + * Sampling a signal on DC motor and compute + * a new value of speed according with PID control. + */ +static void dc_motor_do(int index) +{ + DCMotor *dcm = &dcm_all[index]; + + dc_speed_t curr_pos; + pwm_duty_t new_pid; + + //If select DC motor is not active we return + if (!(dcm->status & DC_MOTOR_ACTIVE)) + return; + + //Acquire the output signal + curr_pos = ADC_RANGECONV(adc_read(dcm->cfg->adc_ch), dcm->cfg->adc_min, dcm->cfg->adc_max); + + //Compute next value for reaching target speed from current position + new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos); + +//TODO: Add a multilevel debug.. +// if (debug_msg_delay == 20) +// { +// TRACEMSG("DCmt[%d]>curr_speed[%d],tgt[%d],err[%f],P[%f],I[%f],D[%f]", dcm->index, curr_pos, dcm->tgt_speed); +// kprintf("%d,", curr_pos); +// debug_msg_delay = 0; +// } +// debug_msg_delay++; + +// TRACEMSG("tg[%d], new_pid[%d], pos[%d]", dcm->tgt_speed, new_pid, curr_pos); + + //Apply the compute duty value + pwm_setDuty(dcm->cfg->pwm_dev, new_pid); + + //Restart dc motor + pwm_enable(dcm->cfg->pwm_dev, true); + + DC_MOTOR_ENABLE(dcm->index); +} + +/** + * Set spin direction of DC motor. + * + * \a index number of DC motor + * \a dir direction of DC motor + */ +void dc_motor_setDir(int index, bool dir) +{ + DCMotor *dcm = &dcm_all[index]; + + if (dir != (dcm->status & DC_MOTOR_DIR)) + { + //Reset previous direction flag, and set new + dcm->status &= ~DC_MOTOR_DIR; + dcm->status |= DC_MOTOR_DIR; + DC_MOTOR_SET_DIR(index, dir); + } + +} + + +/** + * Set DC motor speed. + */ +void dc_motor_setSpeed(int index, dc_speed_t speed) +{ + dcm_all[index].tgt_speed = speed; +} + +/** + * Enable or disable dc motor + */ +void dc_motor_enable(int index, bool state) +{ + DCMotor *dcm = &dcm_all[index]; + + /* + * Clean all PID stutus variable, becouse + * we start with new one. + */ + pid_control_reset(&dcm->pid_ctx); + + if (state) + { + dcm->status |= DC_MOTOR_ACTIVE; + } + else + { + pwm_enable(dcm->cfg->pwm_dev, false); + DC_MOTOR_DISABLE(dcm->index); + + dcm->status &= ~DC_MOTOR_ACTIVE; + } +} + +/** + * Apply a confinguration to select DC motor. + */ +void dc_motor_setup(int index, DCMotorConfig *dcm_conf) +{ + DCMotor *dcm = &dcm_all[index]; + + dcm->cfg = dcm_conf; + + /* + * Apply config value. + */ + dcm->index = index; + + DC_MOTOR_INIT(dcm->index); + + pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq); + pwm_enable(dcm->cfg->pwm_dev, false); + + //Init pid control + pid_control_init(&dcm->pid_ctx, &dcm->cfg->pid_cfg); + + /* + * We are using the same sample period for each + * motor, and so we check if this value is the same + * for all. The sample period time is defined in pid + * configuration. + * + * TODO: Use a different sample period for each motor + * and refactor a module to allow to use a timer interrupt, + * in this way we can controll a DC motor also without a + * kernel, increasing a portability on other target. + */ + if (!sample_period) + sample_period = dcm->cfg->pid_cfg.sample_period; + + ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period); + + //Set default direction for DC motor + DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir); + + 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); + TRACEMSG(" > pwm_dev[%d], freq[%ld], sample[%ld]", dcm_conf->pwm_dev, dcm_conf->freq, dcm_conf->sample_delay); + TRACEMSG(" > adc_ch[%d], adc_max[%d], adc_min[%d]", dcm_conf->adc_ch, dcm_conf->adc_max, dcm_conf->adc_min); + TRACEMSG(" > dir[%d]", dcm->cfg->dir); + +} + + +/** + * Init DC motor + */ +void dc_motor_init(void) +{ + for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + dcm_all[i].status &= ~DC_MOTOR_ACTIVE; + + //Init a sample period + sample_period = 0; + + //Create a dc motor poll process + proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack); + +} + diff --git a/bertos/drv/dc_motor.h b/bertos/drv/dc_motor.h new file mode 100644 index 00000000..9aac6548 --- /dev/null +++ b/bertos/drv/dc_motor.h @@ -0,0 +1,106 @@ +/** + * \file + * + * + * + * \brief DC motor driver (interface) + * + * \version $Id$ + * + * \author Daniele Basile + */ + +#ifndef DRV_DC_MOTOR_H +#define DRV_DC_MOTOR_H + +#include "hw_dc_motor.h" + +#include + +#include +#include +#include + +#include + +#include "appconfig.h" + +/** + * Define status bit for DC motor device. + */ +#define DC_MOTOR_ACTIVE BV(0) ///< DC motor enable or disable flag. +#define DC_MOTOR_DIR BV(1) ///< Spin direction of DC motor. + +/** + * Type for DC motor. + */ +typedef uint16_t dc_speed_t; + +/** + * DC motor configuration stucture + */ +typedef struct DCMotorConfig +{ + PidCfg pid_cfg; ///< Pid control. + + PwmDev pwm_dev; ///< Pwm channel. + pwm_freq_t freq; ///< Pwm waveform frequency. + + adc_ch_t adc_ch; ///< ADC channel. + adcread_t adc_max; ///< ADC max scale value. + adcread_t adc_min; ///< ADC min scale value. + mtime_t sample_delay; ///< Delay before to sampling. + + bool dir; ///< Default direction for select DC motor. + +} DCMotorConfig; + + +/** + * Context structure for DC motor. + */ +typedef struct DCMotor +{ + const DCMotorConfig *cfg; ///< All configuration for select DC motor. + PidContext pid_ctx; ///< Pid control. + + int index; ///< DC motor id. + uint32_t status; ///< Status of select DC motor + dc_speed_t tgt_speed; ///< Target speed for select DC motor + +} DCMotor; + +void dc_motor_setDir(int index, bool dir); +void dc_motor_enable(int index, bool state); +void dc_motor_setSpeed(int index, dc_speed_t speed); +void dc_motor_setup(int index, DCMotorConfig *cfg); +void dc_motor_init(void); + +#endif /* DRV_DC_MOTOR_H */