File content as of revision 11:4747badb2448:

/**
 * @brief   Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.
 * @file    main.cpp
 * @author  Jan Tetour <jan.tetour@gmail.com>
 *
 * Test application for STM32F4 for small DC motor control. Main specifications:
 *    - DC motor controlled by PWM
 *    - Motor driver used L298N 
 *    - RPM evaluated via simple impulse sensor
 *    - Speed (RPM) controlled by PID controller
 */
 
#include "mbed.h"

#include "FastPWM.h"
#include "FastIO.h"

#include "PID.h"

// Define MODSERIAL buffer sizes
#define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 16
#define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 64 
#include "MODSERIAL.h"

// Pin defintions
#define IMPULSE_SENSOR_R_PIN    (PA_9)
#define PWM_OUT_R_PIN           (PA_6)

// Serial port definitions
MODSERIAL pcLink(SERIAL_TX, SERIAL_RX);

// Tasks timming definitions
static const us_timestamp_t periodImpSens  =      125000; // 125 msec
static const us_timestamp_t periodLEDBlink =      100000; // 100 msec
static const us_timestamp_t periodPWMWrite =      250000; // 250 msec
static const us_timestamp_t periodRPMSetpoint = 10000000; // 10 sec

static us_timestamp_t tStampImpSens  = 0;
static us_timestamp_t tStampLEDBlink = 0;
static us_timestamp_t tStampPWMWrite = 0;
static us_timestamp_t tStampRPMSetpoint = 0;

static us_timestamp_t tStamp = 0;

static Timer myTimer;

// RPM Sensor module level variables
static unsigned int uiImpSens = 0U;
static unsigned int uiImpSensTemp = 0U;
static int          iImpSensLastState = 0;

// PWM Generator module level variables
static float fPwmDuty = 0.0f;

// RPM Controller module level variables
static float fRPMSetpoint = 0.0f;

// LOCAL MODULE VARIABLES
// I/O pins related
FastPWM mypwm(PWM_OUT_R_PIN);
FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn;
FastIn<USER_BUTTON> pinUserButtonIn;
DigitalOut myled(LED1);

// Controllers
PID pid_RPM_Right_motor(0.00175f, 0.00200f, 0.000f, (((float)periodPWMWrite)/1000000.0f));

// LOCAL FUNCTION DECLARATIONS
// Task worker functions
static void tskImpSens(void);
static void tskLEDBlink(void);
static void tskPWMWrite(void);
static void tskRPMSetpoint(void);
static void tskBackground(void);

// Inititalization
static void setup(void);

// Task management functions
static inline void DO_TASK(us_timestamp_t tskPeriod, us_timestamp_t &tskTimer, us_timestamp_t timeStamp, void (*tskFunction)(void))
{
    if (tskPeriod < (timeStamp - tskTimer))
    {
        tskTimer = timeStamp;
        (*tskFunction)();
    }
}

static inline void BACKGROUND(void (*tskFunction)(void))
{
    (*tskFunction)();
}

// Main function definition
int main(void)
{
    setup();
       
    while(1)
    {
        tStamp = myTimer.read_high_resolution_us();
 
        DO_TASK(periodLEDBlink,     tStampLEDBlink,     tStamp, &tskLEDBlink);
        DO_TASK(periodImpSens,      tStampImpSens,      tStamp, &tskImpSens);
        DO_TASK(periodRPMSetpoint,  tStampRPMSetpoint,  tStamp, &tskRPMSetpoint);
        DO_TASK(periodPWMWrite,     tStampPWMWrite,     tStamp, &tskPWMWrite);

        BACKGROUND(&tskBackground);
    }
}


// LOCAL MODULE DEFINITIONS
/**
 * @brief   System initialization - called at the prologue of main().
 * @note    None.
 *
 * Caries out system level initialization at the beginning of the main().
 */
void setup(void)
{
    pcLink.baud(115200);
    pcLink.format(8, SerialBase::None, 1);
    
    mypwm.period_us(3500);
    mypwm.write(0.0);
  
    myTimer.start();
    
    //Analog input from 0.0 to 100.0 impulses per measurement period
    pid_RPM_Right_motor.setInputLimits(16.0f, 35.0f);
    
    //Pwm output from 0.0 to 1.0
    pid_RPM_Right_motor.setOutputLimits(0.05f, 1.0f);
    
    //If there's a bias.
    pid_RPM_Right_motor.setBias(0.0f);
    pid_RPM_Right_motor.setMode(AUTO_MODE);
    
    //We want the process variable to be 0.0 RPM
    fRPMSetpoint = 16.0f;
    pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
}

// Task worker functions definitions
/**
 * @brief   RPM calculation.
 * @note    Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
 *
 * Stores impulse count per measurement period and clears impulse counter.
 */
void tskImpSens(void)
{
    uiImpSens = uiImpSensTemp;
    uiImpSensTemp = 0U;

 //   pcLink.printf("IMP: %u imp.     \r", uiImpSens);
}

/**
 * @brief   User LED flashing.
 *
 * Implements User LED flashing.
 */
void tskLEDBlink(void)
{
    myled = !myled;
}

/**
 * @brief   Writes new duty cycle value into PWM generator.
 * @note    Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
 * @warning Not finished.
 *
 * Calculates new dyty cycle and writes the value into PWM generator.
  */
void tskPWMWrite(void)
{
//    fPwmDuty = fPwmDuty + 0.1;
            
//    if (1.0 < fPwmDuty)
//    {
//        fPwmDuty = 0.0;
//    }

    //Update the process variable.
    pid_RPM_Right_motor.setProcessValue((float)uiImpSens);

    //Set the new output.
    fPwmDuty = pid_RPM_Right_motor.compute();

    mypwm.write(fPwmDuty);

    pcLink.printf("PWM: %.2f %%\tIMP: %.2f imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, (float)uiImpSens, fRPMSetpoint);

//    pcLink.printf("\r\nPWM: %.2f %%     \r\n", mypwm.read() * 100);
}

/**
 * @brief   Implementation of periodic change of RPM Setpoint. Simulates setpoint changes to asses dynamic behaviour.
 * @note    For test purposes.
 *
 * Increases Setpoint value step by step with every invocation.
 */
void tskRPMSetpoint(void)
{
    fRPMSetpoint += 2.0f;
    if (35.0f < fRPMSetpoint)
    {
        fRPMSetpoint = 16.0f;
    }

    pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
}

/**
 * @brief   Implementation of background task. Periodically called from main loop without delay.
 * @note    These actions are candidates for refactoring to event based implementation.
 * @warning Initial implementation. Simple and easy.
 *
 * This function implements actions, which needs to be processed with high priority.
 * Is intended to be called in every pass of main loop.
 */
void tskBackground(void)
{
    // Impulse sensor - pulse counting
    int iTemp = pinImpulseSensorIn.read();
    if (iTemp != iImpSensLastState)
    {
        iImpSensLastState = iTemp;
        uiImpSensTemp++;
    }
}