+
+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;
+}