X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fdrv%2Fdc_motor.c;h=34cc56bd31ae524827f5bf1c277c9e9b5a74466d;hb=ed4b778f8193e5c3252db7c217c31eec6db834c5;hp=1df0083e7a95c0c9150d929c653b13e552b916a2;hpb=674d5052e41cd7fc5ce3d05ac44e7b543cd34016;p=bertos.git diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c index 1df0083e..34cc56bd 100644 --- a/bertos/drv/dc_motor.c +++ b/bertos/drv/dc_motor.c @@ -34,47 +34,151 @@ * * 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 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 + * 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 + * 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_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_VERBOSITY DC_MOTOR_LOG_FORMAT + +#include +#include #include #include -#include - -#include #include -#define DC_MOTOR_TARGET_TRIM 0 +#include + +/** + * 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 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]; +static int dcm_registered_num; -/// Stack process for DC motor poll. -static cpustack_t dc_motor_poll_stack[200]; +// Stack process for DC motor poll. +static PROC_DEFINE_STACK(dc_motor_poll_stack, 400); -///< Sample period for all DC motor. +// Sample period for all DC motor. static mtime_t sample_period; // Only for Debug -// static int debug_msg_delay = 0; +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); +} + +/** + * Read the target speed from select device. + */ +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); + +} + + +/* + * 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 = 0; + pwm_duty_t new_pid; + + //If select DC motor is not active we return + if (!(dcm->status & DC_MOTOR_ACTIVE)) + return; + + + //Compute next value for reaching target speed from current position + if (dcm->cfg->pid_enable) + { + curr_pos = dc_motor_readSpeed(index); + 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; + } + debug_msg_delay++; + kputs("\n");); + + //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); +} + + +/* + * 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) +{ + 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; + } -//Forward declaration -static void dc_motor_do(int index); + return true; +} /** * Process to poll DC motor status. @@ -101,17 +205,30 @@ static void NORETURN dc_motor_poll(void) * 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++) + for (i = 0; i < dcm_registered_num; i++) { - dc_motor_do(i); - max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay); + if (check_timerIsExpired(i)) + { + 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) + dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i)); } //Wait for next sampling timer_delay(sample_period - max_sample_delay); - for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++) + for (i = 0; i < dcm_registered_num; i++) { + check_timerIsExpired(i); + if (dcm_all[i].status & DC_MOTOR_ACTIVE) { DC_MOTOR_DISABLE(dcm_all[i].index); @@ -121,67 +238,9 @@ static void NORETURN dc_motor_poll(void) //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. * @@ -192,14 +251,13 @@ 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); - } - + /* + * 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_SET_STATUS_DIR(dcm->status, dir); } @@ -208,18 +266,42 @@ void dc_motor_setDir(int index, bool dir) */ void dc_motor_setSpeed(int index, dc_speed_t speed) { - dcm_all[index].tgt_speed = speed; + DCMotor *dcm = &dcm_all[index]; + + dcm->tgt_speed = speed; + + LOG_INFO("DC Motor[%d]: Tspeed[%d]\n", index, dcm->tgt_speed); } /** - * Enable or disable dc motor + * 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) + dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time); + } + +/** + * 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) +void dc_motor_enable(int index, bool state, int mode) { DCMotor *dcm = &dcm_all[index]; /* - * Clean all PID stutus variable, becouse + * Clean all PID stutus variable, becouse * we start with new one. */ pid_control_reset(&dcm->pid_ctx); @@ -231,9 +313,12 @@ void dc_motor_enable(int index, bool state) else { pwm_enable(dcm->cfg->pwm_dev, false); - DC_MOTOR_DISABLE(dcm->index); - dcm->status &= ~DC_MOTOR_ACTIVE; + + if (mode == DC_MOTOR_DISABLE_MODE) + DC_MOTOR_DISABLE(dcm->index); + else /* DC_MOTOR_IDLE_MODE */ + DC_MOTOR_IDLE(dcm->index); } } @@ -251,7 +336,18 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) */ dcm->index = index; - DC_MOTOR_INIT(dcm->index); + /* + * By default the motor run forever.. + */ + dcm->expire_time = DC_MOTOR_NO_EXPIRE; + + /* + * Clear the status. + */ + 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); @@ -277,28 +373,40 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) //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); + 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("> 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); } /** - * Init DC motor + * Init DC motor. + * \a priority: sets the dc motor process priority. */ -void dc_motor_init(void) +void dc_motor_init(int priority) { - for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++) - dcm_all[i].status &= ~DC_MOTOR_ACTIVE; + ASSERT(CONFIG_KERN); - //Init a sample period + 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(); + //Create a dc motor poll process - proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack); + 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); }