main.cpp

00001 /**
00002  * @brief   Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.
00003  * @file    main.cpp
00004  * @author  Jan Tetour <jan.tetour@gmail.com>
00005  *
00006  * Test application for STM32F4 for small DC motor control. Main specifications:
00007  *    - DC motor controlled by PWM
00008  *    - Motor driver used L298N 
00009  *    - RPM evaluated via simple impulse sensor
00010  *    - Speed (RPM) controlled by PID controller
00011  */
00012  
00013 #include "mbed.h"
00014 
00015 #include "FastPWM.h"
00016 #include "FastIO.h"
00017 
00018 #include "PID.h"
00019 
00020 // Define MODSERIAL buffer sizes
00021 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 16
00022 #define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 64 
00023 #include "MODSERIAL.h"
00024 
00025 // Pin defintions
00026 #define IMPULSE_SENSOR_R_PIN    (PA_9)
00027 #define PWM_OUT_R_PIN           (PA_6)
00028 
00029 // Serial port definitions
00030 MODSERIAL pcLink(SERIAL_TX, SERIAL_RX);
00031 
00032 // Tasks timming definitions
00033 static const us_timestamp_t periodImpSens  =      125000; // 125 msec
00034 static const us_timestamp_t periodLEDBlink =      100000; // 100 msec
00035 static const us_timestamp_t periodPWMWrite =      250000; // 250 msec
00036 static const us_timestamp_t periodRPMSetpoint = 10000000; // 10 sec
00037 
00038 static us_timestamp_t tStampImpSens  = 0;
00039 static us_timestamp_t tStampLEDBlink = 0;
00040 static us_timestamp_t tStampPWMWrite = 0;
00041 static us_timestamp_t tStampRPMSetpoint = 0;
00042 
00043 static us_timestamp_t tStamp = 0;
00044 
00045 static Timer myTimer;
00046 
00047 // RPM Sensor module level variables
00048 static unsigned int uiImpSens = 0U;
00049 static unsigned int uiImpSensTemp = 0U;
00050 static int          iImpSensLastState = 0;
00051 
00052 // PWM Generator module level variables
00053 static float fPwmDuty = 0.0f;
00054 
00055 // RPM Controller module level variables
00056 static float fRPMSetpoint = 0.0f;
00057 
00058 // LOCAL MODULE VARIABLES
00059 // I/O pins related
00060 FastPWM mypwm(PWM_OUT_R_PIN);
00061 FastIn<IMPULSE_SENSOR_R_PIN> pinImpulseSensorIn;
00062 FastIn<USER_BUTTON> pinUserButtonIn;
00063 DigitalOut myled(LED1);
00064 
00065 // Controllers
00066 PID pid_RPM_Right_motor(0.00175f, 0.00200f, 0.000f, (((float)periodPWMWrite)/1000000.0f));
00067 
00068 // LOCAL FUNCTION DECLARATIONS
00069 // Task worker functions
00070 static void tskImpSens(void);
00071 static void tskLEDBlink(void);
00072 static void tskPWMWrite(void);
00073 static void tskRPMSetpoint(void);
00074 static void tskBackground(void);
00075 
00076 // Inititalization
00077 static void setup(void);
00078 
00079 // Task management functions
00080 static inline void DO_TASK(us_timestamp_t tskPeriod, us_timestamp_t &tskTimer, us_timestamp_t timeStamp, void (*tskFunction)(void))
00081 {
00082     if (tskPeriod < (timeStamp - tskTimer))
00083     {
00084         tskTimer = timeStamp;
00085         (*tskFunction)();
00086     }
00087 }
00088 
00089 static inline void BACKGROUND(void (*tskFunction)(void))
00090 {
00091     (*tskFunction)();
00092 }
00093 
00094 // Main function definition
00095 int main(void)
00096 {
00097     setup();
00098        
00099     while(1)
00100     {
00101         tStamp = myTimer.read_high_resolution_us();
00102  
00103         DO_TASK(periodLEDBlink,     tStampLEDBlink,     tStamp, &tskLEDBlink);
00104         DO_TASK(periodImpSens,      tStampImpSens,      tStamp, &tskImpSens);
00105         DO_TASK(periodRPMSetpoint,  tStampRPMSetpoint,  tStamp, &tskRPMSetpoint);
00106         DO_TASK(periodPWMWrite,     tStampPWMWrite,     tStamp, &tskPWMWrite);
00107 
00108         BACKGROUND(&tskBackground);
00109     }
00110 }
00111 
00112 
00113 // LOCAL MODULE DEFINITIONS
00114 /**
00115  * @brief   System initialization - called at the prologue of main().
00116  * @note    None.
00117  *
00118  * Caries out system level initialization at the beginning of the main().
00119  */
00120 void setup(void)
00121 {
00122     pcLink.baud(115200);
00123     pcLink.format(8, SerialBase::None, 1);
00124     
00125     mypwm.period_us(3500);
00126     mypwm.write(0.0);
00127   
00128     myTimer.start();
00129     
00130     //Analog input from 0.0 to 100.0 impulses per measurement period
00131     pid_RPM_Right_motor.setInputLimits(16.0f, 35.0f);
00132     
00133     //Pwm output from 0.0 to 1.0
00134     pid_RPM_Right_motor.setOutputLimits(0.05f, 1.0f);
00135     
00136     //If there's a bias.
00137     pid_RPM_Right_motor.setBias(0.0f);
00138     pid_RPM_Right_motor.setMode(AUTO_MODE);
00139     
00140     //We want the process variable to be 0.0 RPM
00141     fRPMSetpoint = 16.0f;
00142     pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
00143 }
00144 
00145 // Task worker functions definitions
00146 /**
00147  * @brief   RPM calculation.
00148  * @note    Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
00149  *
00150  * Stores impulse count per measurement period and clears impulse counter.
00151  */
00152 void tskImpSens(void)
00153 {
00154     uiImpSens = uiImpSensTemp;
00155     uiImpSensTemp = 0U;
00156 
00157  //   pcLink.printf("IMP: %u imp.     \r", uiImpSens);
00158 }
00159 
00160 /**
00161  * @brief   User LED flashing.
00162  *
00163  * Implements User LED flashing.
00164  */
00165 void tskLEDBlink(void)
00166 {
00167     myled = !myled;
00168 }
00169 
00170 /**
00171  * @brief   Writes new duty cycle value into PWM generator.
00172  * @note    Needs refactoring to implement #ifdef for 2 different implementation (with/without PID).
00173  * @warning Not finished.
00174  *
00175  * Calculates new dyty cycle and writes the value into PWM generator.
00176   */
00177 void tskPWMWrite(void)
00178 {
00179 //    fPwmDuty = fPwmDuty + 0.1;
00180             
00181 //    if (1.0 < fPwmDuty)
00182 //    {
00183 //        fPwmDuty = 0.0;
00184 //    }
00185 
00186     //Update the process variable.
00187     pid_RPM_Right_motor.setProcessValue((float)uiImpSens);
00188 
00189     //Set the new output.
00190     fPwmDuty = pid_RPM_Right_motor.compute();
00191 
00192     mypwm.write(fPwmDuty);
00193 
00194     pcLink.printf("PWM: %.2f %%\tIMP: %.2f imp.\tSET: %.2f imp.\t\r", mypwm.read() * 100, (float)uiImpSens, fRPMSetpoint);
00195 
00196 //    pcLink.printf("\r\nPWM: %.2f %%     \r\n", mypwm.read() * 100);
00197 }
00198 
00199 /**
00200  * @brief   Implementation of periodic change of RPM Setpoint. Simulates setpoint changes to asses dynamic behaviour.
00201  * @note    For test purposes.
00202  *
00203  * Increases Setpoint value step by step with every invocation.
00204  */
00205 void tskRPMSetpoint(void)
00206 {
00207     fRPMSetpoint += 2.0f;
00208     if (35.0f < fRPMSetpoint)
00209     {
00210         fRPMSetpoint = 16.0f;
00211     }
00212 
00213     pid_RPM_Right_motor.setSetPoint(fRPMSetpoint);
00214 }
00215 
00216 /**
00217  * @brief   Implementation of background task. Periodically called from main loop without delay.
00218  * @note    These actions are candidates for refactoring to event based implementation.
00219  * @warning Initial implementation. Simple and easy.
00220  *
00221  * This function implements actions, which needs to be processed with high priority.
00222  * Is intended to be called in every pass of main loop.
00223  */
00224 void tskBackground(void)
00225 {
00226     // Impulse sensor - pulse counting
00227     int iTemp = pinImpulseSensorIn.read();
00228     if (iTemp != iImpSensLastState)
00229     {
00230         iImpSensLastState = iTemp;
00231         uiImpSensTemp++;
00232     }
00233 }