From: asterix Date: Tue, 4 May 2010 10:52:56 +0000 (+0000) Subject: Refactor all module. Clean up. Fix comments. Some patch to work with preemtive kernel... X-Git-Tag: 2.5.0~297 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=b9c96114f83504fc6492d7aad95aaeb5d68532ac;p=bertos.git Refactor all module. Clean up. Fix comments. Some patch to work with preemtive kernel. Add hw test for dc motor. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@3611 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/cfg/cfg_dc_motor.h b/bertos/cfg/cfg_dc_motor.h index ce45e365..fd9bac73 100644 --- a/bertos/cfg/cfg_dc_motor.h +++ b/bertos/cfg/cfg_dc_motor.h @@ -53,7 +53,7 @@ * $WIZ$ type = "enum" * $WIZ$ value_list = "log_level" */ -#define DC_MOTOR_LOG_LEVEL LOG_LVL_WARN +#define DC_MOTOR_LOG_LEVEL LOG_LVL_INFO /** * Module logging format. @@ -61,7 +61,7 @@ * $WIZ$ type = "enum" * $WIZ$ value_list = "log_format" */ -#define DC_MOTOR_LOG_FORMAT LOG_FMT_TERSE +#define DC_MOTOR_LOG_FORMAT LOG_FMT_VERBOSE /** @@ -85,4 +85,27 @@ #define CONFIG_DC_MOTOR_MAX_SPEED 65535 +/** + * Sampling period in millisecond. + * $WIZ$ type = "int" + */ +#define CONFIG_DC_MOTOR_SAMPLE_PERIOD 40 + +/** + * Amount of millisecond before to read sample. + * $WIZ$ type = "int" + */ +#define CONFIG_DC_MOTOR_SAMPLE_DELAY 2 + +/** + * This control set which mode the driver use to lock share + * resources when we use the preempitive kernel. + * If we set to 1 we use the semaphore module otherwise the + * driver disable the switch context every time we need to access + * to shared sources. + * + * $WIZ$ type = "int" + */ +#define CONFIG_DC_MOTOR_USE_SEM 1 + #endif /* CFG_DC_MOTOR_H */ diff --git a/bertos/drv/dc_motor.c b/bertos/drv/dc_motor.c index 34cc56bd..13d8b769 100644 --- a/bertos/drv/dc_motor.c +++ b/bertos/drv/dc_motor.c @@ -34,13 +34,12 @@ * * 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 */ @@ -50,7 +49,7 @@ // 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,6 +60,8 @@ #include +#include + #include /** @@ -81,18 +82,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_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 +119,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 +128,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->cfg->pwm_dev, 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,30 +187,52 @@ 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)) + 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_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 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); @@ -160,6 +241,7 @@ static void dc_motor_do(int index) pwm_enable(dcm->cfg->pwm_dev, true); DC_MOTOR_ENABLE(dcm->index); + DC_MOTOR_UNLOCK; } @@ -169,75 +251,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); } + 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 +334,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 +346,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 +401,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; /* @@ -341,72 +431,58 @@ void dc_motor_setup(int index, DCMotorConfig *dcm_conf) */ dcm->expire_time = DC_MOTOR_NO_EXPIRE; + /* + * By default set target speed. + */ + dcm->tgt_speed = dcm_conf->speed; + /* * 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); - //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); 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); - } diff --git a/bertos/drv/dc_motor.h b/bertos/drv/dc_motor.h index 32a0b13c..cf7b4905 100644 --- a/bertos/drv/dc_motor.h +++ b/bertos/drv/dc_motor.h @@ -54,14 +54,8 @@ #include #include -/* - * DC motor mode stop. - */ -#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. +#define DC_MOTOR_NO_EXPIRE -1 ///< The DC motor runs do not expire, so it runs forever. +#define DC_MOTOR_NO_DEV_SPEED -1 ///< Disable the speed acquire from device (like trimmer, etc.). /** * Type for DC motor. @@ -82,14 +76,13 @@ typedef struct DCMotorConfig 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. + bool braked; ///< If true the motor is braked when we turn off it. - dc_speed_t speed; ///< Fixed speed value for select DC motor, if enable_dev_speed flag is false. + dc_speed_t speed; ///< Default speed value for select DC motor. - 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. + int speed_dev_id; ///< Index of the device where read speed, to disable set to DC_MOTOR_NO_DEV_SPEED. } DCMotorConfig; @@ -104,19 +97,30 @@ 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 + ticks_t expire_time; ///< Amount of time that dc motor run } DCMotor; void dc_motor_setDir(int index, bool dir); -void dc_motor_enable(int index, bool state, int mode); +void dc_motor_enable(int index, bool state); 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_startTimer(int index, mtime_t on_time); +void dc_motor_waitStop(int index); +void dc_motor_setup(int index, DCMotorConfig *dcm_conf); dc_speed_t dc_motor_readTargetSpeed(int index); -void dc_motor_init(int priority); +void dc_motor_setPriority(int priority); +void dc_motor_init(void); + + +/** + * Test function prototypes. + * + * See dc_motor_hwtest.c file. + */ +int dc_motor_testSetUp(void); +void dc_motor_testRun(void); +int dc_motor_testTearDown(void); #endif /* DRV_DC_MOTOR_H */ diff --git a/bertos/drv/dc_motor_hwtest.c b/bertos/drv/dc_motor_hwtest.c new file mode 100644 index 00000000..477d35aa --- /dev/null +++ b/bertos/drv/dc_motor_hwtest.c @@ -0,0 +1,178 @@ +/** + * \file + * + * + * + * \brief Test for PWM driver (implementation) + * + * This is a simple test for PWM driver. This module + * is target independent, so you can test all target that + * BeRTOS support. + * To use this test you should include a pwm_map.h header where + * are defined the PWM channels for your target. Then you should add + * or remove a test setting in pwm_test_cfg array, and edit a value for + * your specific test. + * Afther this, all is ready and you can test PWM driver. + * + * The test check first if all PWM channel starts, and then try + * to change a PWM duty cicle for all channel. + * The change of duty cycle is operate when a PWM channel is enable, + * in this way you can see if a pwm signal is clean and work properly. + * The duty value is change incrementaly, and when it arrive to 100% or 0%, + * we reset the duty value and restart the test. + * Further the duty test, we check also a PWM polarity, infact when we + * reach a reset duty value, we invert a polary of PWM wavform. + * So you can see if the hardware manage correctly this situation. + * + * Note: To be simple and target independently we not use a timer module, + * and so the delay is do with a for cycle. + * + * \author Daniele Basile + * + * \brief HW test for DC Motor. + */ + +#include + +#include +// 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 + +#include + +#include + +#include +#include + +static DCMotorConfig motor = +{ + /* PID */ + { + .kp = 1, /* Proportional coefficient */ + .ki = 4, /* Integral coefficient */ + .kd = 0.008, /* Derivate coefficient */ + .i_max = 2E33, /* Integrale max error value */ + .i_min = -2E33, /* Integrale min error value */ + .out_max = 65535, /* Max output value */ + .out_min = 0, /* Min output value */ + .sample_period = 0 /* Millisecod between 2 output singal sampling */ + }, + .pid_enable = true, /* Enable or disable pid control */ + + /* PWM */ + .pwm_dev = 2, /* PWM channel */ + .freq = 3000, /* Frquency of PWM output waveform */ + + /* ADC */ + .adc_ch = 2, /* ADC channel */ + .adc_max = 65535, /* Max range value for ADC */ + .adc_min = 0, /* Min range value for ADC */ + + /* DC Motor */ + .dir = 1, /* Default spin direction of DC motor */ + .braked = true, + + .speed = 10000, /* Fixed speed value for seldc_motor_enableect DC motor, if enable_dev_speed flag is false */ + .speed_dev_id = 7, /* Index of the device where read speed */ +}; + +int dc_motor_testSetUp(void) +{ + IRQ_ENABLE; + kdbg_init(); + timer_init(); + proc_init(); + pwm_init(); + adc_init(); + + return 0; +} + +#define MOTOR 2 + +void NORETURN dc_motor_testRun(void) +{ + dc_motor_init(); + + /* + * Assign the configuration to motor. + */ + dc_motor_setup(MOTOR, &motor); + + while (1) + { + /* + * Using enable and disable + */ + dc_motor_setDir(MOTOR, 1); + dc_motor_setSpeed(MOTOR, 10000); + dc_motor_enable(MOTOR, true); + timer_delay(500); + dc_motor_enable(MOTOR, false); + + + dc_motor_setDir(MOTOR, 0); + dc_motor_setSpeed(MOTOR, 60000); + dc_motor_enable(MOTOR, true); + timer_delay(150); + dc_motor_enable(MOTOR, false); + + /* + * Using timer + */ + dc_motor_setDir(MOTOR, 1); + dc_motor_setSpeed(MOTOR, 60000); + dc_motor_startTimer(MOTOR, 150); + dc_motor_waitStop(MOTOR); + + dc_motor_setDir(MOTOR, 0); + dc_motor_setSpeed(MOTOR, 10000); + dc_motor_startTimer(MOTOR, 500); + dc_motor_waitStop(MOTOR); + } + +} + +int dc_motor_testTearDown(void) +{ + return 0; +} diff --git a/bertos/hw/hw_dc_motor.h b/bertos/hw/hw_dc_motor.h index 4592af99..f39388df 100644 --- a/bertos/hw/hw_dc_motor.h +++ b/bertos/hw/hw_dc_motor.h @@ -57,8 +57,11 @@ // Macro that disable the select DC motor #define DC_MOTOR_DISABLE(dev) /* Implement me! */ +// Macro that left the DC motor rotor float +#define DC_MOTOR_STOP_FLOAT(dev) DC_MOTOR_DISABLE(dev) // Macro that put in short circuit DC motor supply pins -#define DC_MOTOR_IDLE(dev) do { /* Implement me! */ } while (0) +#define DC_MOTOR_STOP_BRAKED(dev) do { /* Implement me! */ } while (0) + // Macro that set motor direction #define DC_MOTOR_SET_DIR(dev, dir) do { /* Implement me! */ } while (0)