From: asterix Date: Tue, 27 Apr 2010 15:20:54 +0000 (+0000) Subject: Some refactor. Clean up. Add comments. X-Git-Tag: 2.5.0~359 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=ed4b778f8193e5c3252db7c217c31eec6db834c5;p=bertos.git Some refactor. Clean up. Add comments. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@3549 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/cfg/cfg_dc_motor.h b/bertos/cfg/cfg_dc_motor.h index 409badda..12a8afd8 100644 --- a/bertos/cfg/cfg_dc_motor.h +++ b/bertos/cfg/cfg_dc_motor.h @@ -32,7 +32,6 @@ * * \brief Configuration file for DC motor module. * - * \version $Id$ * * \author Daniele Basile */ @@ -64,4 +63,26 @@ */ #define DC_MOTOR_LOG_FORMAT LOG_FMT_TERSE + +/** + * Min value of DC motor speed. + * \note Generally this value is the min value of the ADC conversion, + * if you use it. + * + * $WIZ$ type = "int" + */ +#define CONFIG_DC_MOTOR_MIN_SPEED 0 + + +/** + * Min value of DC motor speed. + * \note Generally this value is the min value of the ADC conversion, + * if you use it. + * + * $WIZ$ type = "int" + * $WIZ$ max = 65535 + */ +#define CONFIG_DC_MOTOR_MAX_SPEED 65535 + + #endif /* CFG_DC_MOTOR_H */ diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c index dae5610d..34cc56bd 100644 --- a/bertos/drv/dc_motor.c +++ b/bertos/drv/dc_motor.c @@ -34,8 +34,13 @@ * * 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 */ @@ -53,28 +58,127 @@ #include #include -#include #include #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)) + + /** * DC motor definition. */ static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR]; +static int dcm_registered_num; -/// Stack process for DC motor poll. -static cpu_stack_t dc_motor_poll_stack[1024]; +// 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 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]; + 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; + } + + 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); @@ -124,48 +241,6 @@ static void NORETURN dc_motor_poll(void) } } -/* - * 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); - - - LOG_INFOB(if (debug_msg_delay == 20) - { - LOG_INFO("DCmt[%d]>curr_speed[%d],tgt[%d]\n", dcm->index, curr_pos, dcm->tgt_speed); - LOG_INFO("%d,", curr_pos); - debug_msg_delay = 0; - } - debug_msg_delay++; - kputs("\n");); - - LOG_WARN("tg[%d], new_pid[%d], pos[%d]\n", 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. * @@ -176,19 +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; - - /* - * 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(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); } @@ -197,13 +266,37 @@ 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_enable(int index, bool state) + 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, int mode) { DCMotor *dcm = &dcm_all[index]; @@ -220,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); } } @@ -240,8 +336,20 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) */ dcm->index = 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_setPolarity(dcm->cfg->pwm_dev, dcm->cfg->pol); pwm_enable(dcm->cfg->pwm_dev, false); //Init pid control @@ -265,9 +373,10 @@ 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); - LOG_INFO("DC motort[%d]:\n", dcm->index); - 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); + 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); @@ -276,19 +385,28 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) /** - * 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) { + ASSERT(CONFIG_KERN); + + struct Process *dc_motor; + memset(dcm_all, 0, sizeof(dcm_all)); - //Init a sample period + // 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); } diff --git a/bertos/drv/dc_motor.h b/bertos/drv/dc_motor.h index 9e710403..32a0b13c 100644 --- a/bertos/drv/dc_motor.h +++ b/bertos/drv/dc_motor.h @@ -31,9 +31,7 @@ * * * \brief DC motor driver. - * - * \version $Id$ - * +* * \author Daniele Basile * * $WIZ$ module_name = "dc_motor" @@ -56,12 +54,14 @@ #include #include - -/** - * Define status bit for DC motor device. +/* + * DC motor mode stop. */ -#define DC_MOTOR_ACTIVE BV(0) ///< DC motor enable or disable flag. -#define DC_MOTOR_DIR BV(1) ///< Spin direction of DC motor. +#define DC_MOTOR_DISABLE_MODE 0 ///< Disable the DC motor shutting down the driver +#define DC_MOTOR_IDLE_MODE 1 ///< Put the motor pins in short circuit + + +#define DC_MOTOR_NO_EXPIRE -1 ///< The DC motor runs do not expire, so it runs forever. /** * Type for DC motor. @@ -74,10 +74,10 @@ typedef uint16_t dc_speed_t; typedef struct DCMotorConfig { PidCfg pid_cfg; ///< Pid control. + bool pid_enable; ///< Flag to disable or enable pid control. PwmDev pwm_dev; ///< Pwm channel. pwm_freq_t freq; ///< Pwm waveform frequency. - bool pol; ///< Pwm waveform polarity. adc_ch_t adc_ch; ///< ADC channel. adcread_t adc_max; ///< ADC max scale value. @@ -85,8 +85,11 @@ typedef struct DCMotorConfig mtime_t sample_delay; ///< Delay before to sampling. bool dir; ///< Default direction for select DC motor. - int speed_trm_id; ///< Index of trimmer to set speed. - dc_speed_t speed; ///< Default speed value for select DC motor. + + dc_speed_t speed; ///< Fixed speed value for select DC motor, if enable_dev_speed flag is false. + + adc_ch_t speed_dev_id; ///< Index of the device where read speed. + bool enable_dev_speed; ///< If this flag is true read target speed from device, otherwise use fixed speed. } DCMotorConfig; @@ -101,14 +104,19 @@ typedef struct DCMotor int index; ///< DC motor id. uint32_t status; ///< Status of select DC motor + dc_speed_t zero_speed; ///< Start value for motor speed (Value read from adc when motor is off) dc_speed_t tgt_speed; ///< Target speed for select DC motor + ticks_t expire_time; ///< Among of time that dc motor run + } DCMotor; void dc_motor_setDir(int index, bool dir); -void dc_motor_enable(int index, bool state); +void dc_motor_enable(int index, bool state, int mode); void dc_motor_setSpeed(int index, dc_speed_t speed); +void dc_motor_setTimer(int index, mtime_t on_time); void dc_motor_setup(int index, DCMotorConfig *cfg); -void dc_motor_init(void); +dc_speed_t dc_motor_readTargetSpeed(int index); +void dc_motor_init(int priority); #endif /* DRV_DC_MOTOR_H */ diff --git a/bertos/hw/hw_dc_motor.h b/bertos/hw/hw_dc_motor.h index 8b8dd4b4..4592af99 100644 --- a/bertos/hw/hw_dc_motor.h +++ b/bertos/hw/hw_dc_motor.h @@ -32,54 +32,37 @@ * * \brief DC motor hardware-specific definitions * - * \version $Id$ - * * \author Daniele Basile */ #ifndef HW_DC_MOTOR_H #define HW_DC_MOTOR_H -typedef enum MotorDCMap -{ - - /* Put here motor dc declaration */ - MOTOR_DC_CNT +#warning TODO:This is an example implementation, you must implement it! -} MotorDCMap; - -/* - * Init all pin and device to manage dc motor. +/** + * Define fuctions which read adc value from specific device */ -#define MOTOR_DC_INIT() \ - do { \ - /* Implement me! */ \ - } while (0) + #define HW_DC_MOTOR_READ_VALUE(dev, min, max) \ + ({ \ + /* Put here the fuction that read from ADC */ \ + (void)(dev); \ + (void)(min); \ + (void)(max); \ + (0); \ + }) +// Macro that enable the select DC motor +#define DC_MOTOR_ENABLE(dev) /* Implement me! */ +// Macro that disable the select DC motor +#define DC_MOTOR_DISABLE(dev) /* Implement me! */ -/* - * Enable DC motor. - */ -#define DC_MOTOR_ENABLE(dev) \ - do { \ - /* Implement me! */ \ - } while (0) - -/* - * Disable DC motor. - */ -#define DC_MOTOR_DISABLE(dev) \ - do { \ - /* Implement me! */ \ - } while (0) +// Macro that put in short circuit DC motor supply pins +#define DC_MOTOR_IDLE(dev) do { /* Implement me! */ } while (0) +// Macro that set motor direction +#define DC_MOTOR_SET_DIR(dev, dir) do { /* Implement me! */ } while (0) -/* - * Set direction for DC motor. - */ -#define DC_MOTOR_SET_DIR(dev, dir) \ - do { \ - /* Implement me! */ \ - } while (0) +#define MOTOR_DC_INIT() do { /* Implement me! */ } while (0) #endif /* HW_DC_MOTOR_H */