BeRTOS
|
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