X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fdrv%2Fdc_motor.c;h=3609b7bda77e6dc3d7090ea74c370f77dc077b0e;hb=41718ab2098bd5640da265c34f1ecb79a4123d39;hp=34cc56bd31ae524827f5bf1c277c9e9b5a74466d;hpb=ed4b778f8193e5c3252db7c217c31eec6db834c5;p=bertos.git diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c index 34cc56bd..3609b7bd 100644 --- a/bertos/drv/dc_motor.c +++ b/bertos/drv/dc_motor.c @@ -34,23 +34,23 @@ * * Thi module provide a simple api to controll a DC motor in direction and * speed, to allow this we use a Back-EMF technique. - * This technique is basid on the capability of the DC motor to became generator - * of voltage when we turn off its supply. This happend every time we turn off the - * DC motor supply, and it continue to rotate for a short time thanks its mechanical + * 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 - * going to read the volage value from DC motor supply pins. This voltage say to us + * we read the volage value from DC motor supply pins. This voltage say to us * the actual speed of the motor. - * Sampling the DC motor speed we are able to controll its speed. * * \author Daniele Basile */ #include "dc_motor.h" #include "hw/hw_dc_motor.h" +#include "cfg/cfg_pwm.h" // Define logging setting (for cfg/log.h module). #define LOG_LEVEL DC_MOTOR_LOG_LEVEL -#define LOG_VERBOSITY DC_MOTOR_LOG_FORMAT +#define LOG_FORMAT DC_MOTOR_LOG_FORMAT #include #include @@ -61,8 +61,22 @@ #include +#include + #include +#if CFG_PWM_ENABLE_OLD_API + #define PWM_ENABLE(dcm, en) pwm_enable((dcm)->cfg->pwm_dev, (en)) + #define PWM_SETDUTY(dcm, duty) pwm_setDuty((dcm)->cfg->pwm_dev, (duty)) + #define PWM_SETFREQ(dcm, freq) pwm_setFrequency((dcm)->cfg->pwm_dev, (freq)) + #define PWM_SETPOL(dcm, pol) pwm_setPolarity((dcm)->cfg->pwm_dev, (pol)) +#else + #define PWM_ENABLE(dcm, en) pwm_enable(&(dcm)->pwm, (en)) + #define PWM_SETDUTY(dcm, duty) pwm_setDuty(&(dcm)->pwm, (duty)) + #define PWM_SETFREQ(dcm, freq) pwm_setFrequency(&(dcm)->pwm, (freq)) + #define PWM_SETPOL(dcm, pol) pwm_setPolarity(&(dcm)->pwm, (pol)) +#endif + /** * Define status bit for DC motor device. */ @@ -81,18 +95,35 @@ #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 /** * DC motor definition. */ static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR]; -static int dcm_registered_num; -// Stack process for DC motor poll. -static PROC_DEFINE_STACK(dc_motor_poll_stack, 400); +/* + * 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 LOG_INFOB(static int debug_msg_delay = 0;); @@ -101,8 +132,6 @@ LOG_INFOB(static int debug_msg_delay = 0;); INLINE dc_speed_t dc_motor_readSpeed(int index) { DCMotor *dcm = &dcm_all[index]; - LOG_INFO("DC motor[%d]\n", index); - return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max); } @@ -112,12 +141,55 @@ INLINE dc_speed_t dc_motor_readSpeed(int index) dc_speed_t dc_motor_readTargetSpeed(int index) { DCMotor *dcm = &dcm_all[index]; - LOG_INFO("DC motor[%d]\n", index); - return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED); +} +static void dc_motor_start(int index) +{ + DCMotor *dcm = &dcm_all[index]; + + 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; } +/* + * 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]; + + DC_MOTOR_LOCK; + + dcm->status &= ~DC_MOTOR_ACTIVE; + dcm->expire_time = DC_MOTOR_NO_EXPIRE; + PWM_ENABLE(dcm, false); + + if (dcm->cfg->braked) + { + DC_MOTOR_STOP_BRAKED(dcm->index); + } + else + { + DC_MOTOR_STOP_FLOAT(dcm->index); + } + + DC_MOTOR_UNLOCK; +} /* * Sampling a signal on DC motor and compute @@ -128,38 +200,61 @@ static void dc_motor_do(int index) DCMotor *dcm = &dcm_all[index]; dc_speed_t curr_pos = 0; - pwm_duty_t new_pid; + 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; + } + /* + * To set dc motor direction we must also set the + * PWM polarity according with dc motor driver chip + */ + PWM_SETPOL(dcm, 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 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; - - 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, curr_pos, dcm->tgt_speed); - debug_msg_delay = 0; + new_pid = dcm->tgt_speed; } - debug_msg_delay++; - kputs("\n");); + + 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 - pwm_setDuty(dcm->cfg->pwm_dev, new_pid); + PWM_SETDUTY(dcm, new_pid); //Restart dc motor - pwm_enable(dcm->cfg->pwm_dev, true); + PWM_ENABLE(dcm, true); DC_MOTOR_ENABLE(dcm->index); + DC_MOTOR_UNLOCK; } @@ -169,75 +264,77 @@ static void dc_motor_do(int index) */ INLINE bool check_timerIsExpired(int index) { - if (((dcm_all[index].expire_time - timer_clock()) < 0) && - (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE)) - { - dc_motor_enable(index, false, DC_MOTOR_IDLE_MODE); - dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE; - return false; - } - return true; + 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 max_sample_delay, that value are stored + * 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 max_sample_delay, choose among a max delay + * 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 (sample_period - max_sample_delay) that every time - * we turn off a DC motor is choose to have a feedback controll + * 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 (;;) { - 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 < dcm_registered_num; i++) + 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); - max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay); - } /* * 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->enable_dev_speed) + 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(sample_period - max_sample_delay); + timer_delay(CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY); - for (i = 0; i < dcm_registered_num; i++) + for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) { - check_timerIsExpired(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); + PWM_ENABLE(&dcm_all[i], false); } + DC_MOTOR_UNLOCK; } //Wait some time to allow signal to stabilize before sampling - timer_delay(max_sample_delay); + timer_delay(CONFIG_DC_MOTOR_SAMPLE_DELAY); } } @@ -250,17 +347,11 @@ static void NORETURN dc_motor_poll(void) void dc_motor_setDir(int index, bool dir) { DCMotor *dcm = &dcm_all[index]; - - /* - * To set dc motor direction we must also set the - * PWM polarity according with dc motor driver chip - */ - pwm_setPolarity(dcm->cfg->pwm_dev, dir); - DC_MOTOR_SET_DIR(dcm->index, dir); + DC_MOTOR_LOCK; DC_MOTOR_SET_STATUS_DIR(dcm->status, dir); + DC_MOTOR_UNLOCK; } - /** * Set DC motor speed. */ @@ -268,60 +359,54 @@ void dc_motor_setSpeed(int index, dc_speed_t speed) { DCMotor *dcm = &dcm_all[index]; + DC_MOTOR_LOCK; dcm->tgt_speed = speed; + DC_MOTOR_UNLOCK; - LOG_INFO("DC Motor[%d]: Tspeed[%d]\n", index, dcm->tgt_speed); + LOG_INFO("DC Motor[%d]: tgt_speed[%d]\n", index, dcm->tgt_speed); } /** * Set among of time that dc motor should run. */ - void dc_motor_setTimer(int index, mtime_t on_time) - { - dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE; - if (on_time != DC_MOTOR_NO_EXPIRE) +void dc_motor_startTimer(int index, mtime_t on_time) +{ + DC_MOTOR_LOCK; + dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE; + if (on_time != DC_MOTOR_NO_EXPIRE) + { dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time); - } + dc_motor_start(index); + } + DC_MOTOR_UNLOCK; +} -/** - * Enable or disable dc motor. - * - * There are two \a mode to disable 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. - */ -void dc_motor_enable(int index, bool state, int mode) +void dc_motor_waitStop(int index) { DCMotor *dcm = &dcm_all[index]; + bool loop = true; - /* - * 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 + while (loop) { - pwm_enable(dcm->cfg->pwm_dev, false); - dcm->status &= ~DC_MOTOR_ACTIVE; + DC_MOTOR_LOCK; + loop = dcm->status & DC_MOTOR_ACTIVE; + DC_MOTOR_UNLOCK; - if (mode == DC_MOTOR_DISABLE_MODE) - DC_MOTOR_DISABLE(dcm->index); - else /* DC_MOTOR_IDLE_MODE */ - DC_MOTOR_IDLE(dcm->index); + 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. */ @@ -329,6 +414,24 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) { DCMotor *dcm = &dcm_all[index]; + DC_MOTOR_LOCK; + /* + * 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. + */ + 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); + + dcm->cfg = dcm_conf; /* @@ -342,71 +445,59 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) dcm->expire_time = DC_MOTOR_NO_EXPIRE; /* - * Clear the status. + * By default set target speed. */ - dcm->status = 0; - - // Update registered motors. - dcm_registered_num++; - - 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); + dcm->tgt_speed = dcm_conf->speed; /* - * 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. + * Clear the status. */ - if (!sample_period) - sample_period = dcm->cfg->pid_cfg.sample_period; - - ASSERT(sample_period == dcm->cfg->pid_cfg.sample_period); + dcm->status = 0; +#if !CFG_PWM_ENABLE_OLD_API + pwm_init(&dcm->pwm, dcm_conf->pwm_dev); +#endif + PWM_SETFREQ(dcm, dcm->cfg->freq); + PWM_ENABLE(dcm, 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); + 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[%ld]\n", dcm->cfg->pwm_dev, dcm->cfg->freq, dcm->cfg->sample_delay); + 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. * \a priority: sets the dc motor process priority. */ -void dc_motor_init(int priority) +void dc_motor_init(void) { ASSERT(CONFIG_KERN); - struct Process *dc_motor; - - memset(dcm_all, 0, sizeof(dcm_all)); - - // Init a sample period - sample_period = 0; - - // Count how much motor we have to manage. - dcm_registered_num = 0; - MOTOR_DC_INIT(); + #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); - proc_setPri(dc_motor, priority); - }