tolgadurudogan
Published © GPL3+

Tricks for Controlling DC Motors

On this sample I would like to explain why PID-control should be used for speed controls and how the direction can be inverted.

IntermediateFull instructions provided35,314
Tricks for Controlling DC Motors

Things used in this project

Hardware components

Arduino Due
Arduino Due
Actually you can use any Arduino Board.
×1
Makeblock Me TFT LCD
This is an optional component. You can use another type of display, or prefer not to use any.
×1
Resistor 4.75k ohm
Resistor 4.75k ohm
Resistors may vary depending on your implementation.
×4
Rotary potentiometer (generic)
Rotary potentiometer (generic)
3 of them are optional (just to adjust controller coefficients).
×4
General Purpose Transistor PNP
General Purpose Transistor PNP
Transistors to be used may vary depending on your implementation.
×4
DC motor (generic)
×1
Slide Switch
Slide Switch
This is used for direction selection.
×1
Photoelectric Speed Sensor HC-020K
×1
Breadboard (generic)
Breadboard (generic)
×1
Jumper wires (generic)
Jumper wires (generic)
×1

Software apps and online services

Arduino IDE
Arduino IDE

Story

Read more

Schematics

Schematic for Control of Direction and Speed of a DC Motor

It's a prototype to explain DC motor speed control by using PID controller, and what should be considered for reversing.

Code

Code with Graphical Touch-Ups

Arduino
/* ###############################################
 Color constants for Makeblock Me TFT LCD
############################################### */
#define _BLACK  0
#define _RED    1
#define _GREEN  2
#define _BLUE   3
#define _YELLOW 4
#define _CYAN   5
#define _PINK   6
#define _WHITE  7

/* ###############################################
 I/O Assignments
############################################### */
int _chSpeedSet    = A0,  // Speed setpoint
    _chKp          = A1,  // Proportional coefficient reading for PID controller
    _chKi          = A2,  // Integral coefficient reading for PID controller
    _chKd          = A3,  // Derivative coefficient reading for PID controller
    _chMotorCmdCCW = 3,   // PWM output to motor for counter-clockwise turn
    _chMotorCmdCW  = 2,   // PWM output to motor for clockwise turn
    _chSpeedRead   = 24,  // Speed reading
    _chDirection   = 25;  // Direction selector reading

/* ###############################################
 Other Constants 
############################################### */
#define _minRPM        0  // Minimum RPM to initiate direction changing
#define _maxRPM     6000  // Maximum RPM limit
#define _Tmax         90  // Maximum time limit for graphing
#define _DiscSlots    20  // Qty of slots on Index Disc

/* ###############################################
 Global Variables
############################################### */
String  Cartesian_SetupDetails;
boolean Direction, prevDirection;
// Alarm Settings
float RALL=500.0, RAL=1000.0, RAH=4000.0, RAHH=4500.0;
float Seconds=0.0, prevSeconds=0.0, 
      prevRPM=0.0, prevRPMset=0.0,
      RPM=0.0,     RPMset=0.0, OutputRPM=0.0,
      Kp=0.0,    Ki=0.0,       Kd=0.0, 
      Kpmax=2.0, Kimax=1.0,    Kdmax=1.0,
      E=0.0,     Eprev=0.0,    dT=1.0;

/* ###############################################
 CommandToTFT(TFTCmd)
 Command Function for Makeblock Me TFT LCD
 Input Parameters:
 (String) TFTCmd : Command string
############################################### */
void CommandToTFT(String TFTCmd)
{
  /* Serial Connection used for display */  
  Serial1.println(TFTCmd); delay(5);
}
/* ########### End of CommandToTFT() ########### */
/* ############################################# */

/* ###############################################
 Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY)
 Cartesian X-Y Axis Drawing Function for Makeblock Me TFT LCD
 Input Parameters:
 (float) Xmin, Xmax, Ymin, Ymax : Axis range values
 (int)   Window_X1, Window_Y1___: Upper-left corner of graph window
 (int)   Window_X2, Window_Y2___: Lower-right corner of graph window
 (int)   MinDashQty_____________: Qty.of dashes on shortest axis
 (int)   ColorB, ColorX, ColorY : Drawing colors for Frame, X-axis and Y-axis
 Uses external function CommandToTFT().
############################################### */
String Cartesian_Setup(
     float Xmin, float Xmax, float Ymin, float Ymax, 
     int Window_X1, int Window_Y1, int Window_X2, int Window_Y2, 
     int MinDashQty, int ColorF, int ColorX, int ColorY
     )
{
    /* Screen Limitations */ 
    const int DisplayResolutionX = 319, DisplayResolutionY = 239;
    /* Limit Title Strings */
                  String XminTxt;
                  if (abs(Xmin)>=1000000000)        XminTxt = "X=" + String (Xmin/1000000000) + "G";
                     else if (abs(Xmin)>=1000000)   XminTxt = "X=" + String (Xmin/1000000) + "M";
                          else if (abs(Xmin)>=1000) XminTxt = "X=" + String (Xmin/1000) + "K";
                               else                 XminTxt = "X=" + String (Xmin);
                  String XmaxTxt;
                  if (abs(Xmax)>=1000000000)        XmaxTxt = "X=" + String (Xmax/1000000000) + "G";
                     else if (abs(Xmax)>=1000000)   XmaxTxt = "X=" + String (Xmax/1000000) + "M";
                          else if (abs(Xmax)>=1000) XmaxTxt = "X=" + String (Xmax/1000) + "K";
                               else                 XmaxTxt = "X=" + String (Xmax);
                  String YminTxt;
                  if (abs(Ymin)>=1000000000)        YminTxt = "Y=" + String (Ymin/1000000000) + "G";
                     else if (abs(Ymin)>=1000000)   YminTxt = "Y=" + String (Ymin/1000000) + "M";
                          else if (abs(Ymin)>=1000) YminTxt = "Y=" + String (Ymin/1000) + "K";
                               else                 YminTxt = "Y=" + String (Ymin);
                  String YmaxTxt;
                  if (abs(Ymax)>=1000000000)        YmaxTxt = "Y=" + String (Ymax/1000000000) + "G";
                     else if (abs(Ymax)>=1000000)   YmaxTxt = "Y=" + String (Ymax/1000000) + "M";
                          else if (abs(Ymax)>=1000) YmaxTxt = "Y=" + String (Ymax/1000) + "K";
                               else                 YmaxTxt = "Y=" + String (Ymax);
    /* Limits */  int XminPx = Window_X1+1; int XmaxPx = Window_X2-1; 
                  int YmaxPx = Window_Y1+1; int YminPx = Window_Y2-1;
    /* Origin */  int OriginX = XminPx + (int)( (XmaxPx - XminPx) * abs(Xmin) / (abs(Xmax)+abs(Xmin)) );
                  int OriginY = YmaxPx + (int)( (YminPx - YmaxPx) * abs(Ymax) / (abs(Ymax)+abs(Ymin)) );
    /* Frame */   CommandToTFT ( "BOX(" + String(Window_X1) + "," + String(Window_Y1)+ "," + 
                                          String(Window_X2) + "," + String(Window_Y2)+ "," + 
                                          String(ColorF)    + ");"
                               );
    /* X Axis */  CommandToTFT ( "PL(" + String(Window_X1+1) + "," + String(OriginY) + "," + 
                                         String(Window_X2-1) + "," + String(OriginY) + "," + 
                                         String(ColorX)            + ");" 
                               );
    /* Y Axis */  CommandToTFT ( "PL(" + String(OriginX) + "," + String(Window_Y1+1) + "," + 
                                         String(OriginX) + "," + String(Window_Y2-1) + "," + 
                                         String(ColorY)            + ");"
                               );

    /* 
     Dashing: Minimum amount of dashes is given by "MinDashQty" and will be dashed on the shortest 
     axis-side with respect to origin.
     On the other sections, dashes to be marked shall be determined by considering ratio to 
     shortest axis-side.
     */
    /* Dashing */  int XlengthLeft  = abs(XminPx-OriginX);   int XlengthRight = abs(XmaxPx-OriginX);
                   int YlengthLower = abs(YminPx-OriginY);   int YlengthUpper = abs(YmaxPx-OriginY);
                   int XlengthLeft_Mod, XlengthRight_Mod, YlengthLower_Mod, YlengthUpper_Mod;
                   if (XlengthLeft<=1)  XlengthLeft_Mod=32767;  else XlengthLeft_Mod=XlengthLeft;
                   if (XlengthRight<=1) XlengthRight_Mod=32767; else XlengthRight_Mod=XlengthRight;
                   if (YlengthLower<=1) YlengthLower_Mod=32767; else YlengthLower_Mod=YlengthLower;
                   if (YlengthUpper<=1) YlengthUpper_Mod=32767; else YlengthUpper_Mod=YlengthUpper;
                   int MinAxisLength = min ( min (XlengthLeft_Mod,XlengthRight_Mod), min (YlengthLower_Mod,YlengthUpper_Mod) );
                   int XdashesLeft  = MinDashQty * XlengthLeft / MinAxisLength;
                   int XdashesRight = MinDashQty * XlengthRight / MinAxisLength;
                   int YdashesLower = MinDashQty * YlengthLower / MinAxisLength;
                   int YdashesUpper = MinDashQty * YlengthUpper / MinAxisLength;
                   int DashingInterval=2; // Min.interval btw.dashes
    
    /* X-Dash L */  DashingInterval  = (int) (XlengthLeft / XdashesLeft);
                    if (!(DashingInterval<2))
                       for (int i=OriginX; i>=XminPx; i-=DashingInterval)
                          CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 
                                                 String(i) + "," + String(OriginY+2) + "," + 
                                                 String(ColorX)  + ");"
                                       );
    /* X-Dash R */  DashingInterval  = (int) (XlengthRight / XdashesRight);
                    if (!(DashingInterval<2))
                       for (int i=OriginX; i<=XmaxPx; i+=DashingInterval)
                           CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 
                                                  String(i) + "," + String(OriginY+2) + "," + 
                                                  String(ColorX)  + ");"
                                        );
    /* Y-Dash-L */  DashingInterval  = (int) (YlengthLower / YdashesLower);
                    if (!(DashingInterval<2))
                       for (int i=OriginY; i<=YminPx; i+=DashingInterval)
                           CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 
                                                  String(OriginX+2) + "," + String(i) + "," + 
                                                  String(ColorY)  + ");"
                                        );
    /* Y-Dash-U */  DashingInterval  = (int) (YlengthUpper / YdashesUpper);
                    if (!(DashingInterval<2))
                       for (int i=OriginY; i>=YmaxPx; i-=DashingInterval)
                           CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 
                                                  String(OriginX+2) + "," + String(i) + "," + 
                                                  String(ColorY)  + ");"
                                        );

    /* Calculating coordinates to display axis endpoint values */
    int XminTxtX = Window_X1 - (int)(XminTxt.length()*6) - 1,
        XminTxtY = OriginY, 
        XmaxTxtX = Window_X2 + 1,
        XmaxTxtY = OriginY, 
        YminTxtX = OriginX,
        YminTxtY = Window_Y2 + 1,
        YmaxTxtX = OriginX,
        YmaxTxtY = Window_Y1 - 12 - 1;
    /*  Controls: If any coordinate is -1, it shall fall beyond display limits 
        and respective value shall not be displayed */
    if (XminTxtX<0) XminTxtX = -1;
    if ( (XminTxtY-12) < 0 ) XminTxtY = -1;
    if ( (XmaxTxtX+6*XmaxTxt.length()) > DisplayResolutionX ) XmaxTxtX = -1;
    if ( (XmaxTxtY+12) > DisplayResolutionY ) XmaxTxtY = -1;
    if ( (YminTxtX+6*YminTxt.length()) > DisplayResolutionX ) YminTxtX = -1;
    if ( (YminTxtY+12) > DisplayResolutionY ) YminTxtY = -1;
    if ( (YmaxTxtX+6*YmaxTxt.length()) > DisplayResolutionX ) YmaxTxtX = -1;
    if (YmaxTxtY<0) YmaxTxtY = -1;

     /* Range Limit Titles */
     if ( ( XminTxtX != -1 ) && ( XminTxtY != -1 ) )
        CommandToTFT( "DS12(" + String(XminTxtX) + "," + String(XminTxtY) + ",'" + String(XminTxt) + "'," + String(ColorX) + ");" );
     if ( ( XmaxTxtX != -1 ) && ( XmaxTxtY != -1 ) )
        CommandToTFT( "DS12(" + String(XmaxTxtX) + "," + String(XmaxTxtY) + ",'" + String(XmaxTxt) + "'," + String(ColorX) + ");" );
     if ( ( YminTxtX != -1 ) && ( YminTxtY != -1 ) )
        CommandToTFT( "DS12(" + String(YminTxtX) + "," + String(YminTxtY) + ",'" + String(YminTxt) + "'," + String(ColorY) + ");" );
     if ( ( YmaxTxtX != -1 ) && ( YmaxTxtY != -1 ) )
        CommandToTFT( "DS12(" + String(YmaxTxtX) + "," + String(YmaxTxtY) + ",'" + String(YmaxTxt) + "'," + String(ColorY) + ");" );

    /* 
       Return Value String
       Cartesian_Setup() will return a string packing graphic configuration in following format:
       "<Xmin,Xmax,Ymin,Ymax,Window_X1,Window_Y1,Window_X2,Window_Y2>"
       String starts with '<' and ends by '>'. Each value is delimited by ','  
    */ 
    /* Initialize    */ String Cartesian_SetupDetails = "<";
                        Cartesian_SetupDetails += ( String(Xmin) + "," );
                        Cartesian_SetupDetails += ( String(Xmax) + "," );
                        Cartesian_SetupDetails += ( String(Ymin) + "," );
                        Cartesian_SetupDetails += ( String(Ymax) + "," );
                        Cartesian_SetupDetails += ( String(Window_X1) + "," );
                        Cartesian_SetupDetails += ( String(Window_Y1) + "," );
                        Cartesian_SetupDetails += ( String(Window_X2) + "," );
                        Cartesian_SetupDetails += ( String(Window_Y2) + "," );
    /* Close-Out     */ Cartesian_SetupDetails += ">";

return Cartesian_SetupDetails;
}
/* ########### End of Cartesian_Setup() ########### */
/* ################################################ */

/* ###############################################
 Cartesian_ClearPlotAreas(Descriptor, Color)
 Plot Area Reset/Clear Function for Makeblock Me TFT LCD
 Input Parameters:
 (String) Descriptor : Setup Descriptor - returned by Cartesian_Setup()
 (int)    Color______: Color to be used to fill plot area
 Uses external function CommandToTFT().
############################################### */
void Cartesian_ClearPlotAreas(String Descriptor, int Color)
{
  int X1,Y1,X2,Y2;      /* Boundary coordinates for plot areas */
  /* Extracting values from Descriptor */
  /* L[0] L[1] L[2] L[3] W[0]      W[1]      W[2]      W[3]       */
  /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2  */
  float L[4]; int W[4]; /* Values stored in Descriptor */
  int j=0;              /* Counter */
  String D_Str = "";
  for (int i=1; i<=(Descriptor.length()-1); i++)
      if ( Descriptor[i] == ',' )
         { 
           if (j<4) L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt();
           D_Str=""; j++; 
         }
         else
         D_Str += Descriptor[i];

  /* Origin */  int OriginX = (W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / (abs(L[1])+abs(L[0])) );
                int OriginY = (W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2])) );

  /* Clearing Plot Areas */  
  //Area.1 : X+ Y+
  X1 = OriginX + 2 ; Y1 = W[1] + 1 ;
  X2 = W[2] - 1    ; Y2 = OriginY - 2 ;
  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 
                           String(X2) + "," + String(Y2) + "," +
                           String(Color)     + ");"
               );
  //Area.2 : X- Y+
  X1 = W[0] + 1 ;    Y1 = W[1] + 1 ;
  X2 = OriginX - 2 ; Y2 = OriginY - 2 ;
  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 
                           String(X2) + "," + String(Y2) + "," +
                           String(Color)     + ");"
               );
  //Area.3 : X- Y-
  X1 = W[0] + 1 ;    Y1 = OriginY + 2 ;
  X2 = OriginX - 2 ; Y2 = W[3] - 1 ;
  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 
                           String(X2) + "," + String(Y2) + "," +
                           String(Color)     + ");"
               );
  //Area.4 : X+ Y-
  X1 = OriginX + 2 ; Y1 = OriginY + 2 ;
  X2 = W[2] - 1 ;    Y2 = W[3] - 1 ;
  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + 
                           String(X2) + "," + String(Y2) + "," +
                           String(Color)     + ");"
               );
} 
/* ########### End of Cartesian_ClearPlotAreas() ########### */
/* ######################################################### */

/* ###############################################
 Cartesian_Line(Xp, Yp, X, Y, Descriptor, Color)
 Cartesian Line Function for Makeblock Me TFT LCD
 Input Parameters:
 (int)    Xp, Yp_____: Previous plot coordinates - y value vs x
 (int)    X, Y_______: Current plot coordinates - y value vs x
 (String) Descriptor : Setup Descriptor - returned by Cartesian_Setup()
 (int)    Color______: Marking color to be used on (x,y)
 Uses external function CommandToTFT().
############################################### */
void Cartesian_Line(float Xp, float Yp, float X, float Y, String Descriptor, int Color)
{
  /* Extracting values from Descriptor */
  /* L[0] L[1] L[2] L[3] W[0]      W[1]      W[2]      W[3]       */
  /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2  */
  float L[4]; int W[4]; /* Values stored in Descriptor */
  int j=0;              /* Counter */
  String D_Str = "";
  for (int i=1; i<=(Descriptor.length()-1); i++)
      if ( Descriptor[i] == ',' )
         { 
           if (j<4) L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt();
           D_Str=""; j++; 
         }
         else
         D_Str += Descriptor[i];

  /* Origin */  int OriginX = (W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / (abs(L[1])+abs(L[0])) );
                int OriginY = (W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2])) );

  int XminPx = W[0] + 1;
  int XmaxPx = W[2] - 1;
  int YmaxPx = W[1] + 1;
  int YminPx = W[3] - 1;
  
  if (Y>L[3]) Y=L[3];
  if (Y<L[2]) Y=L[2];
    
  float RatioX = (float)( XmaxPx - XminPx ) / ( L[1] - L[0] );
  float RatioY = (float)abs( YmaxPx - YminPx ) / ( L[3] - L[2] );

  int DispXp = XminPx + (int)( RatioX * ( Xp - L[0] ) );
  int DispYp = YminPx - (int)( RatioY * ( Yp - L[2] ) );
  int DispX = XminPx + (int)( RatioX * ( X - L[0] ) );
  int DispY = YminPx - (int)( RatioY * ( Y - L[2] ) );
  if (!(
      ( ( DispXp >= (OriginX-2) ) && ( DispXp <= (OriginX+2) ) ) ||
      ( ( DispYp >= (OriginY-2) ) && ( DispYp <= (OriginY+2) ) ) ||
      ( ( DispX >= (OriginX-2) ) && ( DispX <= (OriginX+2) ) ) ||
      ( ( DispY >= (OriginY-2) ) && ( DispY <= (OriginY+2) ) )
      ))
       CommandToTFT( "PL(" + String(DispXp) + "," + String(DispYp) + "," + String(DispX) + "," + String(DispY) + "," + String(Color) + ");" );
}
/* ########### End of Cartesian_Line() ########### */
/* ############################################### */

/* ###############################################
 readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed)
 Frequency Reading Function
 Input Parameters:
 (int)   _DI_FrequencyCounter_Pin : Digital pin to be read
 (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1)

 Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted.
         It cannot be 0(zero), negative values or a value greater than 10.
         When _ReadingSpeed changed, 1 second shall be divided by this value to calculate
         required counting duration. For example;
          - _ReadingSpeed = 0.1 -> input shall be counted during 10 seconds (=1/0.1)
          - _ReadingSpeed = 0.5 -> input shall be counted during 2 seconds (=1/0.5)
          - _ReadingSpeed = 2.0 -> input shall be counted during 0.5 seconds (=1/2)
          - _ReadingSpeed = 4.0 -> input shall be counted during 0.25 seconds (=1/4)
         Importantly note that, increasing of _ReadingSpeed is a disadvantage especially
         on lower frequencies (generally below 100 Hz) since counting error increases 
         up to 20%~40% by decreasing frequency.
############################################### */

int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
{
  pinMode(_DI_FrequencyCounter_Pin,INPUT);
  byte _DigitalRead, _DigitalRead_Previous = 0;
  unsigned long _Time = 0, _Time_Init;
  float _Frequency = 0;
  if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
     else
       {
         _Time_Init = micros();
         do
          {
           _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin);
           if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++;
           _DigitalRead_Previous = _DigitalRead;
           _Time = micros();
          }
         while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) );
       }
  return (_ReadingSpeed * _Frequency);
}
/* ########### End of readFrequency() ########### */
/* ############################################## */

/* ###############################################
 controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd)
 PID Controller Function
 Input Parameters:
 (float) RangeMin: Minimum limit for output
 (float) RangeMax: Maximum limit for output
 (float) _E_____: Current error signal
 (float) _Eprev : Previous error signal
 (float) _dT____: Time difference as seconds
 (float) _Kp____: Proportional coefficient
 (float) _Ki____: Integral coefficient
 (float) _Kp____: Derivative coefficient
 Adjustment procedure:
   1. Set Kp=0, Ki=0, Kd=0.
   2. Start to increase Kp until the system oscillates at fixed period (Pc) and note 
      critical gain Kc = Kp.
   3. Adjust final coefficients as follows.
      for P-control only  : Kp = 0.50*Kc
      for PI-control only : Kp = 0.45*Kc, Ki = 1.2/Pc
      for PID-control     : Kp = 0.60*Kc, Ki = 2.0/Pc, Kd=Pc/8
   4. Fine tuning could be done by slightly changing each coefficient.
############################################### */
 
float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd)
{
  float P, I, D;
  /*
   Base Formula: U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
   */
  P = _Kp * _E;                             /* Proportional Component */
  I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT;  /* Integral Component */
  D = _Kp * _Kd * (_E-_Eprev) / _dT;        /* Derivative Component */
  return (P+I+D);
}
/* ########### End of controllerPID() ########### */
/* ############################################## */

/* ###############################################
 Setup
############################################### */

void setup()
{
  Serial1.begin(9600);
  Serial1.println("CLS(0);");delay(20);
  analogReadResolution(12);

  pinMode(_chDirection,INPUT);    // Direction selector reading
  pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn
  pinMode(_chMotorCmdCW,OUTPUT);  // PWM output to motor for clockwise turn

  // Initial killing the PWM outputs to motor
  analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
      
  // Initial reading for direction selection
  Direction=digitalRead(_chDirection);   // HIGH=CCW, LOW=CW
  prevDirection=Direction;
  
  // The section below prepares TFT LCD
  // Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY)
  Cartesian_SetupDetails = Cartesian_Setup(0, _Tmax, _minRPM, _maxRPM, 20, 20, 220, 120, 10, 0, 7, 7);
  CommandToTFT("DS12(250,10,'Dir: CW '," + String(_WHITE) + ");");
  CommandToTFT("DS12(250,25,'____ Set'," + String(_YELLOW) + ");");
  CommandToTFT("DS12(250,40,'____ RPM'," + String(_GREEN) + ");");
  /* Alarm Values */
  CommandToTFT("DS12(250,55,'AHH:" + String(RAHH) + "'," + String(_WHITE) + ");");
  CommandToTFT("DS12(250,70,'AH :" + String(RAH)  + "'," + String(_WHITE) + ");");
  CommandToTFT("DS12(250,85,'AL :" + String(RAL)  + "'," + String(_WHITE) + ");");
  CommandToTFT("DS12(250,100,'ALL:"+ String(RALL) + "'," + String(_WHITE) + ");");
  /* Alarm Window */
  CommandToTFT("BOX(240,55,319,115," + String(_WHITE) + ");");
  /* Alarm Lamps */
  CommandToTFT("BOX(240,55,248,70," + String(_WHITE) + ");");
  CommandToTFT("BOX(240,70,248,85," + String(_WHITE) + ");");
  CommandToTFT("BOX(240,85,248,100," + String(_WHITE) + ");");
  CommandToTFT("BOX(240,100,248,115," + String(_WHITE) + ");");
}


/* ###############################################
 Loop
############################################### */

void loop()
{
  
  // Initialization Time: Necessary for PID controller.
        int InitTime = micros();
        
  // X-Axis Auto-Reset for Graphing
        if ( Seconds > 90.0 ) 
          {
            Seconds = 0.0;
            Cartesian_ClearPlotAreas(Cartesian_SetupDetails,0);
          }
          
  // Reading Inputs
     /* Controller Coefficients */
        Kp = Kpmax * (float)analogRead(_chKp) / 4095;
        Ki = Kimax * (float)analogRead(_chKi) / 4095;
        Kd = Kdmax * (float)analogRead(_chKd) / 4095;
     /* Direction Selector */   
        Direction = digitalRead(_chDirection);  /* HIGH=CCW, LOW=CW */ 
     /* Actual RPM and RPM Setpoint 
        Note that maximum selectable RPM is 5000. */
        RPM    = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots;
        RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095;


  // Calculations and Actions
     /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */
        E = RPMset - RPM;
        float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
        if ( RPMset == 0 ) OutputRPM = 0;
           else OutputRPM = OutputRPM + cPID; 
        if ( OutputRPM < _minRPM ) OutputRPM = _minRPM;
        if ( OutputRPM > _maxRPM ) OutputRPM = _maxRPM;
                    
     /* Changing Direction when inverted 
        Note that no any graphical indication is performed on this function.  */
        if ( Direction != prevDirection )
          {
            /* Killing both of the PWM outputs to motor */
               analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
            /* Wait until motor speed decreases */
               do 
                 { RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; }
               while ( RPM > _minRPM );
          }
  
  // Writing Outputs
        if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); 
                        else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM));

  // Graphing
        /* Indicating Direction */
           if (Direction==HIGH) CommandToTFT("DS12(280,10,'CCW '," + String(_WHITE) + ");");
                           else CommandToTFT("DS12(280,10,'CW  '," + String(_WHITE) + ");");
        /* Plotting Curve */
           Cartesian_Line(prevSeconds, prevRPMset, Seconds, RPMset, Cartesian_SetupDetails, _YELLOW);
           Cartesian_Line(prevSeconds, prevRPM, Seconds, RPM, Cartesian_SetupDetails, _GREEN);
        /* Indicating values of RPM Setpoint, PID Controller Coefficients, 
           Error Signal, PID Controller Output and Final RPM Output (PWM) */
           CommandToTFT( "DS12(20,150,'Set: " + String(RPMset) + " rpm  " + 
                                      "RPM: " + String(RPM)    + " rpm  '," + String(_WHITE) + ");");
           CommandToTFT( "DS12(20,170,'Kp= " + String(Kp) + " " +
                                      "Ki= " + String(Ki) + " " +
                                      "Kd= " + String(Kd) + " " +
                                      "dT= " + String(dT*1000) + " ms   '," + String(_WHITE) + ");");
           CommandToTFT( "DS12(20,190,'e= "      + String(E)         + " " +
                                      "cPID= "   + String(cPID)      + " " +
                                      "RPMout= " + String(OutputRPM) + "         '," + String(_WHITE) + ");");
        /* Resetting Alarm Lamps */ 
           CommandToTFT("BOXF(241,56,247,69," + String(_BLACK) + ");");
           CommandToTFT("BOXF(241,71,247,84," + String(_BLACK) + ");");
           CommandToTFT("BOXF(241,86,247,99," + String(_BLACK) + ");");
           CommandToTFT("BOXF(241,101,247,114," + String(_BLACK) + ");");
        /* Activating Necessary Alarm Lamps */ 
           if (RPM>=RAHH)              CommandToTFT("BOXF(241,56,247,69," + String(_RED) + ");");
           if ((RPM>=RAH)&&(RPM<RAHH)) CommandToTFT("BOXF(241,71,247,84," + String(_RED) + ");");
           if ((RPM>RALL)&&(RPM<=RAL)) CommandToTFT("BOXF(241,86,247,99," + String(_RED) + ");");
           if (RPM<=RALL)              CommandToTFT("BOXF(241,101,247,114," + String(_RED) + ");");

  // Storing Values generated on previous cycle
        Eprev = E; prevRPMset = RPMset;   prevRPM = RPM; 
        prevSeconds = Seconds; prevDirection = Direction;

  // Calculating control application cycle time and passed Seconds
        dT = float ( micros() - InitTime ) / 1000000.0;
        Seconds+=dT; 
}

Code without Graphical Touch-Ups

Arduino
/* ###############################################
 I/O Assignments
############################################### */
int _chSpeedSet    = A0,  // Speed setpoint
    _chKp          = A1,  // Proportional coefficient reading for PID controller
    _chKi          = A2,  // Integral coefficient reading for PID controller
    _chKd          = A3,  // Derivative coefficient reading for PID controller
    _chMotorCmdCCW = 3,   // PWM output to motor for counter-clockwise turn
    _chMotorCmdCW  = 2,   // PWM output to motor for clockwise turn
    _chSpeedRead   = 24,  // Speed reading
    _chDirection   = 25;  // Direction selector reading

/* ###############################################
 Other Constants 
############################################### */
#define _minRPM        0  // Minimum RPM to initiate direction changing
#define _maxRPM     6000  // Maximum RPM limit
#define _DiscSlots    20  // Qty of slots on Index Disc

/* ###############################################
 Global Variables
############################################### */
boolean Direction, prevDirection;
float RPM=0.0,     RPMset=0.0, OutputRPM=0.0,
      Kp=0.0,    Ki=0.0,       Kd=0.0, 
      Kpmax=2.0, Kimax=1.0,    Kdmax=1.0,
      E=0.0,     Eprev=0.0,    dT=1.0;

/* ###############################################
 readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed)
 Frequency Reading Function
 Input Parameters:
 (int)   _DI_FrequencyCounter_Pin : Digital pin to be read
 (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1)

 Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted.
         It cannot be 0(zero), negative values or a value greater than 10.
         When _ReadingSpeed changed, 1 second shall be divided by this value to calculate
         required counting duration. For example;
          - _ReadingSpeed = 0.1 -> input shall be counted during 10 seconds (=1/0.1)
          - _ReadingSpeed = 0.5 -> input shall be counted during 2 seconds (=1/0.5)
          - _ReadingSpeed = 2.0 -> input shall be counted during 0.5 seconds (=1/2)
          - _ReadingSpeed = 4.0 -> input shall be counted during 0.25 seconds (=1/4)
         Importantly note that, increasing of _ReadingSpeed is a disadvantage especially
         on lower frequencies (generally below 100 Hz) since counting error increases 
         up to 20%~40% by decreasing frequency.
############################################### */

int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
{
  pinMode(_DI_FrequencyCounter_Pin,INPUT);
  byte _DigitalRead, _DigitalRead_Previous = 0;
  unsigned long _Time = 0, _Time_Init;
  float _Frequency = 0;
  if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
     else
       {
         _Time_Init = micros();
         do
          {
           _DigitalRead = digitalRead(_DI_FrequencyCounter_Pin);
           if ( (_DigitalRead_Previous==1) && (_DigitalRead==0) ) _Frequency++;
           _DigitalRead_Previous = _DigitalRead;
           _Time = micros();
          }
         while ( _Time < (_Time_Init + (1000000/_ReadingSpeed)) );
       }
  return (_ReadingSpeed * _Frequency);
}
/* ########### End of readFrequency() ########### */
/* ############################################## */

/* ###############################################
 controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd)
 PID Controller Function
 Input Parameters:
 (float) RangeMin: Minimum limit for output
 (float) RangeMax: Maximum limit for output
 (float) _E_____: Current error signal
 (float) _Eprev : Previous error signal
 (float) _dT____: Time difference as seconds
 (float) _Kp____: Proportional coefficient
 (float) _Ki____: Integral coefficient
 (float) _Kp____: Derivative coefficient
 Adjustment procedure:
   1. Set Kp=0, Ki=0, Kd=0.
   2. Start to increase Kp until the system oscillates at fixed period (Pc) and note 
      critical gain Kc = Kp.
   3. Adjust final coefficients as follows.
      for P-control only  : Kp = 0.50*Kc
      for PI-control only : Kp = 0.45*Kc, Ki = 1.2/Pc
      for PID-control     : Kp = 0.60*Kc, Ki = 2.0/Pc, Kd=Pc/8
   4. Fine tuning could be done by slightly changing each coefficient.
############################################### */
 
float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd)
{
  float P, I, D;
  /*
   Base Formula: U = _Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
   */
  P = _Kp * _E;                             /* Proportional Component */
  I = _Kp * 0.5 * _Ki * (_E+_Eprev) * _dT;  /* Integral Component */
  D = _Kp * _Kd * (_E-_Eprev) / _dT;        /* Derivative Component */
  return (P+I+D);
}
/* ########### End of controllerPID() ########### */
/* ############################################## */

/* ###############################################
 Setup
############################################### */

void setup()
{
  analogReadResolution(12);
  pinMode(_chDirection,INPUT);    // Direction selector reading
  pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn
  pinMode(_chMotorCmdCW,OUTPUT);  // PWM output to motor for clockwise turn
  // Initial killing the PWM outputs to motor
  analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
  // Initial reading for direction selection
  Direction=digitalRead(_chDirection);   // HIGH=CCW, LOW=CW
  prevDirection=Direction;
}


/* ###############################################
 Loop
############################################### */

void loop()
{
  // Initialization Time: Necessary for PID controller.
        int InitTime = micros();

  // Reading Inputs
     /* Controller Coefficients */
        Kp = Kpmax * (float)analogRead(_chKp) / 4095;
        Ki = Kimax * (float)analogRead(_chKi) / 4095;
        Kd = Kdmax * (float)analogRead(_chKd) / 4095;
     /* Direction Selector */   
        Direction = digitalRead(_chDirection);  /* HIGH=CCW, LOW=CW */ 
     /* Actual RPM and RPM Setpoint 
        Note that maximum selectable RPM is 5000. */
        RPM    = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots;
        RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095;

  // Calculations and Actions
     /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */
        E = RPMset - RPM;
        float cPID = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
        if ( RPMset == 0 ) OutputRPM = 0;
           else OutputRPM = OutputRPM + cPID; 
        if ( OutputRPM < _minRPM ) OutputRPM = _minRPM;
        if ( OutputRPM > _maxRPM ) OutputRPM = _maxRPM;
                    
     /* Changing Direction when inverted */
        if ( Direction != prevDirection )
          {
            /* Killing both of the PWM outputs to motor */
               analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
            /* Wait until motor speed decreases */
               do 
                 { RPM = 60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; }
               while ( RPM > _minRPM );
          }
  
  // Writing Outputs
        if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); 
                        else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM));

  // Storing Values generated on previous cycle
        Eprev = E; prevDirection = Direction;

  // Calculating control application cycle time and passed Seconds
        dT = float ( micros() - InitTime ) / 1000000.0;
}

Credits

tolgadurudogan

tolgadurudogan

4 projects • 23 followers

Comments