X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fdrv%2Fdc_motor.c;h=bd0dd0a465d7feafe115bfa8cab64daa758f8009;hb=1c11ac0ab0636d07db3899b02c5d89e2d0b020bc;hp=1df0083e7a95c0c9150d929c653b13e552b916a2;hpb=674d5052e41cd7fc5ce3d05ac44e7b543cd34016;p=bertos.git diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c index 1df0083e..bd0dd0a4 100644 --- a/bertos/drv/dc_motor.c +++ b/bertos/drv/dc_motor.c @@ -34,29 +34,68 @@ * * 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.. + * This technique is based, on the capability of the DC motor to become a generator + * of voltage when we turn off its supply. This happen every time we turn off the + * DC motor supply, and it continues to rotate for a short time thanks to its mechanical + * energy. Using this idea we can turn off the motor for a very short time, and + * we read the volage value from DC motor supply pins. This voltage say to us + * the actual speed of the motor. * * \author Daniele Basile */ #include "dc_motor.h" -#include "hw_dc_motor.h" +#include "hw/hw_dc_motor.h" + +// Define logging setting (for cfg/log.h module). +#define LOG_LEVEL DC_MOTOR_LOG_LEVEL +#define LOG_FORMAT DC_MOTOR_LOG_FORMAT + +#include +#include #include #include -#include - -#include #include -#define DC_MOTOR_TARGET_TRIM 0 +#include + +#include -#if DC_MOTOR_TARGET_TRIM - #define DC_MOTOR_TARGET_TRIM_SAMPLE_PER 50 - static int tgt_sample_period = 0; +/** + * 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. + +/* + * Some utility macro for motor directions + */ +#define POS_DIR 1 +#define NEG_DIR 0 +#define DC_MOTOR_POS_DIR(x) ((x) |= DC_MOTOR_DIR) // Set directions status positive +#define DC_MOTOR_NEG_DIR(x) ((x) &= ~DC_MOTOR_DIR) // Set directions status negative + +// Update the status with current direction +#define DC_MOTOR_SET_STATUS_DIR(status, dir) \ + (dir == POS_DIR ? DC_MOTOR_POS_DIR(status) : DC_MOTOR_NEG_DIR(status)) + +#if (CONFIG_KERN && CONFIG_KERN_PREEMPT) + #if CONFIG_DC_MOTOR_USE_SEM + #include + + Semaphore dc_motor_sem; + #define DC_MOTOR_LOCK sem_obtain(&dc_motor_sem) + #define DC_MOTOR_UNLOCK sem_release(&dc_motor_sem) + #else + #define DC_MOTOR_LOCK proc_forbid() + #define DC_MOTOR_UNLOCK proc_permit() + #endif +#else + #define DC_MOTOR_LOCK /* None */ + #define DC_MOTOR_UNLOCK /* None */ #endif /** @@ -64,81 +103,79 @@ */ static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR]; -/// Stack process for DC motor poll. -static cpustack_t dc_motor_poll_stack[200]; +/* + * Process to poll dc motor status + */ +struct Process *dc_motor; + -///< Sample period for all DC motor. -static mtime_t sample_period; +// Stack process for DC motor poll. +static PROC_DEFINE_STACK(dc_motor_poll_stack, 500); // Only for Debug -// static int debug_msg_delay = 0; +LOG_INFOB(static int debug_msg_delay = 0;); + -//Forward declaration -static void dc_motor_do(int index); +INLINE dc_speed_t dc_motor_readSpeed(int index) +{ + DCMotor *dcm = &dcm_all[index]; + return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max); +} /** - * 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. + * Read the target speed from select device. */ -static void NORETURN dc_motor_poll(void) +dc_speed_t dc_motor_readTargetSpeed(int index) { - 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); - } + DCMotor *dcm = &dcm_all[index]; + return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED); +} - //Wait for next sampling - timer_delay(sample_period - max_sample_delay); +static void dc_motor_start(int index) +{ + DCMotor *dcm = &dcm_all[index]; - 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); - } - } + DC_MOTOR_LOCK; + /* + * Clean all PID stutus variable, becouse + * we start with new one. + */ + pid_control_reset(&dcm->pid_ctx); + dcm->status |= DC_MOTOR_ACTIVE; + DC_MOTOR_UNLOCK; +} - //Wait some time to allow signal to stabilize before sampling - timer_delay(max_sample_delay); +/* + * There are two \a mode to stop the dc motor: + * - DC_MOTOR_DISABLE_MODE + * - DC_MOTOR_IDLE + * + * The DC_MOTOR_DISABLE_MODE shut down the DC motor and + * leave it floating to rotate. + * The DC_MOTOR_IDLE does not shut down DC motor, but put + * its supply pin in short circuite, in this way the motor result + * braked from intentional rotation. + */ +static void dc_motor_stop(int index) +{ + DCMotor *dcm = &dcm_all[index]; -#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); + DC_MOTOR_LOCK; - 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); + dcm->status &= ~DC_MOTOR_ACTIVE; + dcm->expire_time = DC_MOTOR_NO_EXPIRE; + pwm_enable(dcm->cfg->pwm_dev, false); - dc_motor_setSpeed(i, tgt_speed); - } - tgt_sample_period = 0; - } - tgt_sample_period++; -#endif + if (dcm->cfg->braked) + { + DC_MOTOR_STOP_BRAKED(dcm->index); + } + else + { + DC_MOTOR_STOP_FLOAT(dcm->index); } + + DC_MOTOR_UNLOCK; } /* @@ -149,37 +186,143 @@ static void dc_motor_do(int index) { DCMotor *dcm = &dcm_all[index]; - dc_speed_t curr_pos; - pwm_duty_t new_pid; + dc_speed_t curr_pos = 0; + pwm_duty_t new_pid = 0; + + DC_MOTOR_LOCK; //If select DC motor is not active we return if (!(dcm->status & DC_MOTOR_ACTIVE)) + { + DC_MOTOR_UNLOCK; return; + } - //Acquire the output signal - curr_pos = ADC_RANGECONV(adc_read(dcm->cfg->adc_ch), dcm->cfg->adc_min, dcm->cfg->adc_max); + /* + * To set dc motor direction we must also set the + * PWM polarity according with dc motor driver chip + */ + pwm_setPolarity(dcm->cfg->pwm_dev, dcm->status & DC_MOTOR_DIR); + DC_MOTOR_SET_DIR(dcm->index, dcm->status & DC_MOTOR_DIR); //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++; + if (dcm->cfg->pid_enable) + { + /* + * Here we cannot disable the switch context because the + * driver, that read the speed could be need to use signal or + * other thing that needs the kernel switch context, for this + * reason we unlock before to read the speed. + */ + DC_MOTOR_UNLOCK; + curr_pos = dc_motor_readSpeed(index); + DC_MOTOR_LOCK; + new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos); + } + else + { + new_pid = dcm->tgt_speed; + } -// TRACEMSG("tg[%d], new_pid[%d], pos[%d]", dcm->tgt_speed, new_pid, curr_pos); + LOG_INFOB( + if (debug_msg_delay == 20) + { + LOG_INFO("DC Motor[%d]: curr_speed[%d],curr_pos[%d],tgt[%d]\n", dcm->index, + curr_pos, new_pid, dcm->tgt_speed); + debug_msg_delay = 0; + } + debug_msg_delay++; + ); - //Apply the compute duty value + //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); + DC_MOTOR_UNLOCK; +} + + +/* + * Check if the DC motor run time is expired, if this happend + * we turn off motor and reset status. + */ +INLINE bool check_timerIsExpired(int index) +{ + + DC_MOTOR_LOCK; + bool check = ((dcm_all[index].expire_time - timer_clock()) < 0) && + (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE); + DC_MOTOR_UNLOCK; + + return check; +} + +/** + * Process to poll DC motor status. + * To use a Back-EMF technique (see brief for more details), + * we turn off a motor for CONFIG_DC_MOTOR_SAMPLE_DELAY, that value are stored + * in each DC motor config. For this implementation we assume + * that have a common CONFIG_DC_MOTOR_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 (CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_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 (;;) + { + /* + * For all DC motor we read and process output singal, + * and choose the max value to off time + */ + for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + { + if (!dcm_all[i].cfg) + continue; + + if (check_timerIsExpired(i)) + dc_motor_stop(i); + else + dc_motor_do(i); + + /* + * If we read speed from trimmer we update the target + * speed value when motor is running so we can make + * dc motor speed regulation. + */ + if (dcm_all[i].cfg->speed_dev_id != DC_MOTOR_NO_DEV_SPEED) + dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i)); + } + + //Wait for next sampling + timer_delay(CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY); + + for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + { + if (!dcm_all[i].cfg) + continue; + + if (check_timerIsExpired(i)) + dc_motor_stop(i); + + DC_MOTOR_LOCK; + if (dcm_all[i].status & DC_MOTOR_ACTIVE) + { + DC_MOTOR_DISABLE(dcm_all[i].index); + pwm_enable(dcm_all[i].cfg->pwm_dev, false); + } + DC_MOTOR_UNLOCK; + } + + //Wait some time to allow signal to stabilize before sampling + timer_delay(CONFIG_DC_MOTOR_SAMPLE_DELAY); + } } /** @@ -191,52 +334,66 @@ static void dc_motor_do(int index) 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); - } - + DC_MOTOR_LOCK; + DC_MOTOR_SET_STATUS_DIR(dcm->status, dir); + DC_MOTOR_UNLOCK; } - /** * Set DC motor speed. */ void dc_motor_setSpeed(int index, dc_speed_t speed) { - dcm_all[index].tgt_speed = speed; + DCMotor *dcm = &dcm_all[index]; + + DC_MOTOR_LOCK; + dcm->tgt_speed = speed; + DC_MOTOR_UNLOCK; + + LOG_INFO("DC Motor[%d]: tgt_speed[%d]\n", index, dcm->tgt_speed); } /** - * Enable or disable dc motor + * Set among of time that dc motor should run. */ -void dc_motor_enable(int index, bool state) +void dc_motor_startTimer(int index, mtime_t on_time) { - DCMotor *dcm = &dcm_all[index]; - - /* - * Clean all PID stutus variable, becouse - * we start with new one. - */ - pid_control_reset(&dcm->pid_ctx); - - if (state) + DC_MOTOR_LOCK; + dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE; + if (on_time != DC_MOTOR_NO_EXPIRE) { - dcm->status |= DC_MOTOR_ACTIVE; + dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time); + dc_motor_start(index); } - else + DC_MOTOR_UNLOCK; +} + +void dc_motor_waitStop(int index) +{ + DCMotor *dcm = &dcm_all[index]; + bool loop = true; + + while (loop) { - pwm_enable(dcm->cfg->pwm_dev, false); - DC_MOTOR_DISABLE(dcm->index); + DC_MOTOR_LOCK; + loop = dcm->status & DC_MOTOR_ACTIVE; + DC_MOTOR_UNLOCK; - dcm->status &= ~DC_MOTOR_ACTIVE; + cpu_relax(); } } +/** + * Enable or disable dc motor. + */ +void dc_motor_enable(int index, bool state) +{ + if (state) + dc_motor_start(index); + else + dc_motor_stop(index); +} + /** * Apply a confinguration to select DC motor. */ @@ -244,21 +401,7 @@ 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); - + DC_MOTOR_LOCK; /* * We are using the same sample period for each * motor, and so we check if this value is the same @@ -270,35 +413,76 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) * 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; + pid_control_setPeriod(&dcm_conf->pid_cfg, CONFIG_DC_MOTOR_SAMPLE_PERIOD); + + //Init pid control + pid_control_init(&dcm->pid_ctx, &dcm_conf->pid_cfg); - ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period); + + dcm->cfg = dcm_conf; + + /* + * Apply config value. + */ + dcm->index = index; + + /* + * By default the motor run forever.. + */ + dcm->expire_time = DC_MOTOR_NO_EXPIRE; + + /* + * By default set target speed. + */ + dcm->tgt_speed = dcm_conf->speed; + + /* + * Clear the status. + */ + dcm->status = 0; + + pwm_setFrequency(dcm->cfg->pwm_dev, dcm->cfg->freq); + pwm_enable(dcm->cfg->pwm_dev, false); //Set default direction for DC motor DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir); + DC_MOTOR_SET_STATUS_DIR(dcm->status, 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); + DC_MOTOR_UNLOCK; + LOG_INFO("DC motor[%d]:\n", dcm->index); + 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); + LOG_INFO("> PWM: pwm_dev[%d], freq[%ld], sample[%d]\n", dcm->cfg->pwm_dev, dcm->cfg->freq,CONFIG_DC_MOTOR_SAMPLE_DELAY); + 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); + LOG_INFO("> DC: dir[%d], speed[%d]\n", dcm->cfg->dir, dcm->cfg->speed); } +/** + * If we had enabled the priority scheduling, we can adjust the + * DC motor poll process priority. + */ +void dc_motor_setPriority(int priority) +{ + ASSERT(CONFIG_KERN); + ASSERT(dc_motor); + proc_setPri(dc_motor, priority); +} /** - * Init DC motor + * Init DC motor. + * \a priority: sets the dc motor process priority. */ void dc_motor_init(void) { - for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) - dcm_all[i].status &= ~DC_MOTOR_ACTIVE; + ASSERT(CONFIG_KERN); - //Init a sample period - sample_period = 0; + MOTOR_DC_INIT(); - //Create a dc motor poll process - proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack); + #if (CONFIG_KERN_PREEMPT && CONFIG_DC_MOTOR_USE_SEM) + sem_init(&dc_motor_sem); + #endif + //Create a dc motor poll process + dc_motor = proc_new_with_name("DC_Motor", dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack); }