Basic DC motor control test, rpm feedback by simple impulse signal, PID speed control.
Dependencies: FastPWM mbed FastIO MODSERIAL
main.cpp
- Committer:
- dzoni
- Date:
- 2018-04-04
- Revision:
- 11:4747badb2448
- Parent:
- 10:c28d133a1408
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++; } }