dc_motor.c

Go to the documentation of this file.

#include "dc_motor.h"
#include "hw_dc_motor.h"

#include <algo/pid_control.h>

#include <drv/timer.h>
#include <drv/adc.h>

#include <cfg/debug.h>

#include <kern/proc.h>

#define DC_MOTOR_TARGET_TRIM 0

#if DC_MOTOR_TARGET_TRIM
    #define DC_MOTOR_TARGET_TRIM_SAMPLE_PER 50
    static int tgt_sample_period = 0;
#endif

static DCMotor dcm_all[CONFIG_NUM_DC_MOTOR];

static cpustack_t dc_motor_poll_stack[200];

static mtime_t sample_period;

// Only for Debug
// static int debug_msg_delay = 0;

//Forward declaration
static void dc_motor_do(int index);

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 < CONFIG_NUM_DC_MOTOR; i++)
        {
            dc_motor_do(i);
            max_sample_delay = MAX(max_sample_delay, dcm_all[i].cfg->sample_delay);
        }

        //Wait for next sampling
        timer_delay(sample_period - max_sample_delay);

        for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
        {
            if (dcm_all[i].status & DC_MOTOR_ACTIVE)
            {
                DC_MOTOR_DISABLE(dcm_all[i].index);
                pwm_enable(dcm_all[i].cfg->pwm_dev, false);
            }
        }

        //Wait some time to allow signal to stabilize before sampling
        timer_delay(max_sample_delay);

#if DC_MOTOR_TARGET_TRIM
        if (tgt_sample_period == DC_MOTOR_TARGET_TRIM_SAMPLE_PER)
        {
            dc_speed_t tgt_speed = ADC_RANGECONV(adc_read(6), 0, 65535);

            for (i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
            {
                if ((dcm_all[i].tgt_speed >= tgt_speed - 100) && (dcm_all[i].tgt_speed <= tgt_speed + 100))
                    TRACEMSG("DCmt[%d]> tgt[%d]", i, tgt_speed);

                dc_motor_setSpeed(i, tgt_speed);
            }
            tgt_sample_period = 0;
        }
        tgt_sample_period++;
#endif
    }
}

/*
 * 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;
    pwm_duty_t new_pid;

    //If select DC motor is not active we return
    if (!(dcm->status & DC_MOTOR_ACTIVE))
        return;

    //Acquire the output signal
    curr_pos = ADC_RANGECONV(adc_read(dcm->cfg->adc_ch), dcm->cfg->adc_min, dcm->cfg->adc_max);

    //Compute next value for reaching target speed from current position
    new_pid = pid_control_update(&dcm->pid_ctx, dcm->tgt_speed, curr_pos);

//TODO: Add a multilevel debug..
//  if (debug_msg_delay == 20)
//  {
//      TRACEMSG("DCmt[%d]>curr_speed[%d],tgt[%d],err[%f],P[%f],I[%f],D[%f]", dcm->index, curr_pos, dcm->tgt_speed);
//      kprintf("%d,", curr_pos);
//      debug_msg_delay = 0;
//  }
//  debug_msg_delay++;

//  TRACEMSG("tg[%d], new_pid[%d], pos[%d]", dcm->tgt_speed, new_pid, curr_pos);

    //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);
}

void dc_motor_setDir(int index, bool dir)
{
    DCMotor *dcm = &dcm_all[index];

    if (dir != (dcm->status & DC_MOTOR_DIR))
    {
        //Reset previous direction flag, and set new
        dcm->status &= ~DC_MOTOR_DIR;
        dcm->status |= DC_MOTOR_DIR;
        DC_MOTOR_SET_DIR(index, dir);
    }

}


void dc_motor_setSpeed(int index, dc_speed_t speed)
{
    dcm_all[index].tgt_speed = speed;
}

void dc_motor_enable(int index, bool state)
{
    DCMotor *dcm = &dcm_all[index];

    /*
     * 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
    {
        pwm_enable(dcm->cfg->pwm_dev, false);
        DC_MOTOR_DISABLE(dcm->index);

        dcm->status &= ~DC_MOTOR_ACTIVE;
    }
}

void dc_motor_setup(int index, DCMotorConfig *dcm_conf)
{
    DCMotor *dcm = &dcm_all[index];

    dcm->cfg = dcm_conf;

    /*
     * Apply config value.
     */
    dcm->index = index;

    DC_MOTOR_INIT(dcm->index);

    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);

    TRACEMSG("DC motort[%d]> kp[%f],ki[%f],kd[%f]", dcm->index, dcm_conf->pid_cfg.kp, dcm_conf->pid_cfg.ki, dcm_conf->pid_cfg.kd);
    TRACEMSG("               > pwm_dev[%d], freq[%ld], sample[%ld]", dcm_conf->pwm_dev, dcm_conf->freq, dcm_conf->sample_delay);
    TRACEMSG("               > adc_ch[%d], adc_max[%d], adc_min[%d]", dcm_conf->adc_ch, dcm_conf->adc_max, dcm_conf->adc_min);
    TRACEMSG("               > dir[%d]", dcm->cfg->dir);

}


void dc_motor_init(void)
{
    for (int i = 0; i < CONFIG_NUM_DC_MOTOR; i++)
        dcm_all[i].status &= ~DC_MOTOR_ACTIVE;

    //Init a sample period
    sample_period = 0;

    //Create a dc motor poll process
    proc_new(dc_motor_poll, NULL, sizeof(dc_motor_poll_stack), dc_motor_poll_stack);

}