BeRTOS
dc_motor.c
Go to the documentation of this file.
00001 
00047 #include "dc_motor.h"
00048 #include "hw/hw_dc_motor.h"
00049 #include "cfg/cfg_pwm.h"
00050 
00051 // Define logging setting (for cfg/log.h module).
00052 #define LOG_LEVEL         DC_MOTOR_LOG_LEVEL
00053 #define LOG_FORMAT        DC_MOTOR_LOG_FORMAT
00054 
00055 #include <cfg/log.h>
00056 #include <cfg/debug.h>
00057 
00058 #include <algo/pid_control.h>
00059 
00060 #include <drv/timer.h>
00061 
00062 #include <kern/proc.h>
00063 
00064 #include <cpu/power.h>
00065 
00066 #include <string.h>
00067 
00068 #if CFG_PWM_ENABLE_OLD_API
00069     #define PWM_ENABLE(dcm, en)    pwm_enable((dcm)->cfg->pwm_dev, (en))
00070     #define PWM_SETDUTY(dcm, duty) pwm_setDuty((dcm)->cfg->pwm_dev, (duty))
00071     #define PWM_SETFREQ(dcm, freq) pwm_setFrequency((dcm)->cfg->pwm_dev, (freq))
00072     #define PWM_SETPOL(dcm, pol)   pwm_setPolarity((dcm)->cfg->pwm_dev, (pol))
00073 #else
00074     #define PWM_ENABLE(dcm, en)    pwm_enable(&(dcm)->pwm, (en))
00075     #define PWM_SETDUTY(dcm, duty) pwm_setDuty(&(dcm)->pwm, (duty))
00076     #define PWM_SETFREQ(dcm, freq) pwm_setFrequency(&(dcm)->pwm, (freq))
00077     #define PWM_SETPOL(dcm, pol)   pwm_setPolarity(&(dcm)->pwm, (pol))
00078 #endif
00079 
00083 #define DC_MOTOR_ACTIVE           BV(0)     ///< DC motor enable or disable flag.
00084 #define DC_MOTOR_DIR              BV(1)     ///< Spin direction of DC motor.
00085 
00086 /*
00087  * Some utility macro for motor directions
00088  */
00089 #define POS_DIR                   1
00090 #define NEG_DIR                   0
00091 #define DC_MOTOR_POS_DIR(x)       ((x) |= DC_MOTOR_DIR)   // Set directions status positive
00092 #define DC_MOTOR_NEG_DIR(x)       ((x) &= ~DC_MOTOR_DIR)  // Set directions status negative
00093 
00094 // Update the status with current direction
00095 #define DC_MOTOR_SET_STATUS_DIR(status, dir) \
00096         (dir == POS_DIR ? DC_MOTOR_POS_DIR(status) : DC_MOTOR_NEG_DIR(status))
00097 
00098 #if (CONFIG_KERN && CONFIG_KERN_PREEMPT)
00099     #if CONFIG_DC_MOTOR_USE_SEM
00100         #include <kern/sem.h>
00101 
00102         Semaphore dc_motor_sem;
00103         #define DC_MOTOR_LOCK        sem_obtain(&dc_motor_sem)
00104         #define DC_MOTOR_UNLOCK      sem_release(&dc_motor_sem)
00105     #else
00106         #define DC_MOTOR_LOCK        proc_forbid()
00107         #define DC_MOTOR_UNLOCK      proc_permit()
00108     #endif
00109 #else
00110     #define DC_MOTOR_LOCK        /* None */
00111     #define DC_MOTOR_UNLOCK      /* None */
00112 #endif
00113 
00117 static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];
00118 
00119 /*
00120  * Process to poll dc motor status
00121  */
00122 struct Process *dc_motor;
00123 
00124 
00125 // Stack process for DC motor poll.
00126 static PROC_DEFINE_STACK(dc_motor_poll_stack, 500);
00127 
00128 // Only for Debug
00129 LOG_INFOB(static int debug_msg_delay = 0;);
00130 
00131 
00132 INLINE dc_speed_t dc_motor_readSpeed(int index)
00133 {
00134     DCMotor *dcm = &dcm_all[index];
00135     return HW_DC_MOTOR_READ_VALUE(dcm->cfg->adc_ch, dcm->cfg->adc_min, dcm->cfg->adc_max);
00136 }
00137 
00141 dc_speed_t dc_motor_readTargetSpeed(int index)
00142 {
00143     DCMotor *dcm = &dcm_all[index];
00144     return HW_DC_MOTOR_READ_VALUE(dcm->cfg->speed_dev_id, CONFIG_DC_MOTOR_MIN_SPEED, CONFIG_DC_MOTOR_MAX_SPEED);
00145 }
00146 
00147 static void dc_motor_start(int index)
00148 {
00149     DCMotor *dcm = &dcm_all[index];
00150 
00151     DC_MOTOR_LOCK;
00152     /*
00153      * Clean all PID stutus variable, becouse
00154      * we start with new one.
00155      */
00156     pid_control_reset(&dcm->pid_ctx);
00157     dcm->status |= DC_MOTOR_ACTIVE;
00158     DC_MOTOR_UNLOCK;
00159 }
00160 
00161 /*
00162  * There are two \a mode to stop the dc motor:
00163  *  - DC_MOTOR_DISABLE_MODE
00164  *  - DC_MOTOR_IDLE
00165  *
00166  * The DC_MOTOR_DISABLE_MODE shut down the DC motor and
00167  * leave it floating to rotate.
00168  * The DC_MOTOR_IDLE does not shut down DC motor, but put
00169  * its supply pin in short circuite, in this way the motor result
00170  * braked from intentional rotation.
00171  */
00172 static void dc_motor_stop(int index)
00173 {
00174     DCMotor *dcm = &dcm_all[index];
00175 
00176     DC_MOTOR_LOCK;
00177 
00178     dcm->status &= ~DC_MOTOR_ACTIVE;
00179     dcm->expire_time = DC_MOTOR_NO_EXPIRE;
00180     PWM_ENABLE(dcm, false);
00181 
00182     if (dcm->cfg->braked)
00183     {
00184         DC_MOTOR_STOP_BRAKED(dcm->index);
00185     }
00186     else
00187     {
00188         DC_MOTOR_STOP_FLOAT(dcm->index);
00189     }
00190 
00191     DC_MOTOR_UNLOCK;
00192 }
00193 
00194 /*
00195  * Sampling a signal on DC motor and compute
00196  * a new value of speed according with PID control.
00197  */
00198 static void dc_motor_do(int index)
00199 {
00200     DCMotor *dcm = &dcm_all[index];
00201 
00202     dc_speed_t curr_pos = 0;
00203     pwm_duty_t new_pid = 0;
00204 
00205     DC_MOTOR_LOCK;
00206 
00207     //If select DC motor is not active we return
00208     if (!(dcm->status & DC_MOTOR_ACTIVE))
00209     {
00210         DC_MOTOR_UNLOCK;
00211         return;
00212     }
00213 
00214     /*
00215      * To set dc motor direction we must also set the
00216      * PWM polarity according with dc motor driver chip
00217      */
00218     PWM_SETPOL(dcm, dcm->status & DC_MOTOR_DIR);
00219     DC_MOTOR_SET_DIR(dcm->index, dcm->status & DC_MOTOR_DIR);
00220 
00221     //Compute next value for reaching target speed from current position
00222     if (dcm->cfg->pid_enable)
00223     {
00224         /*
00225          * Here we cannot disable the switch context because the
00226          * driver, that read the speed could be need to use signal or
00227          * other thing that needs the kernel switch context, for this
00228          * reason we unlock before to read the speed.
00229          */
00230         DC_MOTOR_UNLOCK;
00231         curr_pos = dc_motor_readSpeed(index);
00232         DC_MOTOR_LOCK;
00233         new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);
00234     }
00235     else
00236     {
00237         new_pid = dcm->tgt_speed;
00238     }
00239 
00240     LOG_INFOB(
00241         if (debug_msg_delay == 20)
00242         {
00243             LOG_INFO("DC Motor[%d]: curr_speed[%d],curr_pos[%d],tgt[%d]\n", dcm->index,
00244                                 curr_pos, new_pid, dcm->tgt_speed);
00245             debug_msg_delay = 0;
00246         }
00247         debug_msg_delay++;
00248     );
00249 
00250     //Apply the compute duty value
00251     PWM_SETDUTY(dcm, new_pid);
00252 
00253     //Restart dc motor
00254     PWM_ENABLE(dcm, true);
00255 
00256     DC_MOTOR_ENABLE(dcm->index);
00257     DC_MOTOR_UNLOCK;
00258 }
00259 
00260 
00261 /*
00262  * Check if the DC motor run time is expired, if this happend
00263  * we turn off motor and reset status.
00264  */
00265 INLINE bool check_timerIsExpired(int index)
00266 {
00267 
00268     DC_MOTOR_LOCK;
00269     bool check = ((dcm_all[index].expire_time - timer_clock()) < 0) &&
00270             (dcm_all[index].expire_time != DC_MOTOR_NO_EXPIRE);
00271     DC_MOTOR_UNLOCK;
00272 
00273     return check;
00274 }
00275 
00289 static void NORETURN dc_motor_poll(void)
00290 {
00291     for (;;)
00292     {
00293         /*
00294          * For all DC motor we read and process output singal,
00295          * and choose the max value to off time
00296          */
00297         for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
00298         {
00299             if (!dcm_all[i].cfg)
00300                 continue;
00301 
00302             if (check_timerIsExpired(i))
00303                 dc_motor_stop(i);
00304             else
00305                 dc_motor_do(i);
00306 
00307             /*
00308              * If we read speed from trimmer we update the target
00309              * speed value when motor is running so we can make
00310              * dc motor speed regulation.
00311              */
00312             if (dcm_all[i].cfg->speed_dev_id != DC_MOTOR_NO_DEV_SPEED)
00313                 dc_motor_setSpeed(i, dc_motor_readTargetSpeed(i));
00314         }
00315 
00316         //Wait for next sampling
00317         timer_delay(CONFIG_DC_MOTOR_SAMPLE_PERIOD - CONFIG_DC_MOTOR_SAMPLE_DELAY);
00318 
00319         for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
00320         {
00321             if (!dcm_all[i].cfg)
00322                 continue;
00323 
00324             if (check_timerIsExpired(i))
00325                 dc_motor_stop(i);
00326 
00327             DC_MOTOR_LOCK;
00328             if (dcm_all[i].status & DC_MOTOR_ACTIVE)
00329             {
00330                 DC_MOTOR_DISABLE(dcm_all[i].index);
00331                 PWM_ENABLE(&dcm_all[i], false);
00332             }
00333             DC_MOTOR_UNLOCK;
00334         }
00335 
00336         //Wait some time to allow signal to stabilize before sampling
00337         timer_delay(CONFIG_DC_MOTOR_SAMPLE_DELAY);
00338     }
00339 }
00340 
00347 void dc_motor_setDir(int index, bool dir)
00348 {
00349     DCMotor *dcm = &dcm_all[index];
00350     DC_MOTOR_LOCK;
00351     DC_MOTOR_SET_STATUS_DIR(dcm->status, dir);
00352     DC_MOTOR_UNLOCK;
00353 }
00354 
00358 void dc_motor_setSpeed(int index, dc_speed_t speed)
00359 {
00360     DCMotor *dcm = &dcm_all[index];
00361 
00362     DC_MOTOR_LOCK;
00363     dcm->tgt_speed = speed;
00364     DC_MOTOR_UNLOCK;
00365 
00366     LOG_INFO("DC Motor[%d]: tgt_speed[%d]\n", index, dcm->tgt_speed);
00367 }
00368 
00372 void dc_motor_startTimer(int index, mtime_t on_time)
00373 {
00374     DC_MOTOR_LOCK;
00375     dcm_all[index].expire_time = DC_MOTOR_NO_EXPIRE;
00376     if (on_time != DC_MOTOR_NO_EXPIRE)
00377     {
00378         dcm_all[index].expire_time = timer_clock() + ms_to_ticks(on_time);
00379         dc_motor_start(index);
00380     }
00381     DC_MOTOR_UNLOCK;
00382 }
00383 
00384 void dc_motor_waitStop(int index)
00385 {
00386     DCMotor *dcm = &dcm_all[index];
00387     bool loop = true;
00388 
00389     while (loop)
00390     {
00391         DC_MOTOR_LOCK;
00392         loop = dcm->status & DC_MOTOR_ACTIVE;
00393         DC_MOTOR_UNLOCK;
00394 
00395         cpu_relax();
00396     }
00397 }
00398 
00402 void dc_motor_enable(int index, bool state)
00403 {
00404     if (state)
00405         dc_motor_start(index);
00406     else
00407         dc_motor_stop(index);
00408 }
00409 
00413 void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
00414 {
00415     DCMotor *dcm = &dcm_all[index];
00416 
00417     DC_MOTOR_LOCK;
00418     /*
00419      * We are using the same sample period for each
00420      * motor, and so we check if this value is the same
00421      * for all. The sample period time is defined in pid
00422      * configuration.
00423      *
00424      * TODO: Use a different sample period for each motor
00425      * and refactor a module to allow to use a timer interrupt,
00426      * in this way we can controll a DC motor also without a
00427      * kernel, increasing a portability on other target.
00428      */
00429     pid_control_setPeriod(&dcm_conf->pid_cfg, CONFIG_DC_MOTOR_SAMPLE_PERIOD);
00430 
00431     //Init pid control
00432     pid_control_init(&dcm->pid_ctx, &dcm_conf->pid_cfg);
00433 
00434 
00435     dcm->cfg = dcm_conf;
00436 
00437     /*
00438      * Apply config value.
00439      */
00440     dcm->index = index;
00441 
00442     /*
00443      * By default the motor run forever..
00444      */
00445     dcm->expire_time = DC_MOTOR_NO_EXPIRE;
00446 
00447     /*
00448      * By default set target speed.
00449      */
00450     dcm->tgt_speed = dcm_conf->speed;
00451 
00452     /*
00453      * Clear the status.
00454      */
00455     dcm->status = 0;
00456 #if !CFG_PWM_ENABLE_OLD_API
00457     pwm_init(&dcm->pwm, dcm_conf->pwm_dev);
00458 #endif
00459     PWM_SETFREQ(dcm, dcm->cfg->freq);
00460     PWM_ENABLE(dcm, false);
00461 
00462     //Set default direction for DC motor
00463     DC_MOTOR_SET_DIR(dcm->index, dcm->cfg->dir);
00464     DC_MOTOR_SET_STATUS_DIR(dcm->status, dcm->cfg->dir);
00465 
00466     DC_MOTOR_UNLOCK;
00467 
00468     LOG_INFO("DC motor[%d]:\n", dcm->index);
00469     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);
00470     LOG_INFO("> PWM: pwm_dev[%d], freq[%ld], sample[%d]\n", dcm->cfg->pwm_dev, dcm->cfg->freq,CONFIG_DC_MOTOR_SAMPLE_DELAY);
00471     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);
00472     LOG_INFO("> DC: dir[%d], speed[%d]\n", dcm->cfg->dir, dcm->cfg->speed);
00473 }
00474 
00479 void dc_motor_setPriority(int priority)
00480 {
00481     ASSERT(CONFIG_KERN);
00482     ASSERT(dc_motor);
00483     proc_setPri(dc_motor, priority);
00484 }
00485 
00490 void dc_motor_init(void)
00491 {
00492     ASSERT(CONFIG_KERN);
00493 
00494     MOTOR_DC_INIT();
00495 
00496     #if (CONFIG_KERN_PREEMPT && CONFIG_DC_MOTOR_USE_SEM)
00497         sem_init(&dc_motor_sem);
00498     #endif
00499 
00500     //Create a dc motor poll process
00501     dc_motor = proc_new_with_name("DC_Motor", dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);
00502 }
00503