From: asterix <asterix@38d2e660-2303-0410-9eaa-f027e97ec537>
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 <asterix@develer.com>
*/
@@ -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 <cfg/log.h>
#include <cfg/debug.h>
@@ -61,6 +60,8 @@
#include <kern/proc.h>
+#include <cpu/power.h>
+
#include <string.h>
/**
@@ -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 <kern/sem.h>
+
+ 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 <drv/timer.h>
#include <drv/adc.h>
-/*
- * 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
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2005 Develer S.r.l. (http://www.develer.com/)
+ * -->
+ *
+ *
+ * \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 <asterix@develer.com>
+ *
+ * \brief HW test for DC Motor.
+ */
+
+#include <cfg/cfg_dc_motor.h>
+
+#include <cfg/debug.h>
+// Define logging setting (for cfg/log.h module).
+#define LOG_LEVEL DC_MOTOR_LOG_LEVEL
+#define LOG_VERBOSITY DC_MOTOR_LOG_FORMAT
+#include <cfg/log.h>
+
+
+#include <algo/pid_control.h>
+
+#include <drv/timer.h>
+#include <drv/dc_motor.h>
+#include <drv/adc.h>
+#include <drv/pwm.h>
+#include <drv/dc_motor.h>
+
+#include <kern/proc.h>
+
+#include <cpu/irq.h>
+
+#include <verstag.h>
+#include <buildrev.h>
+
+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)