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.

Feb 20, 2018

•

41237 views

•

63 respects

energy efficiency

monitoring

Devices & Components

Arduino Due

Jumper wires (generic)

Photoelectric Speed Sensor HC-020K

Breadboard (generic)

DC motor (generic)

Slide Switch

Rotary potentiometer (generic)

Resistor 4.75k ohm

Makeblock Me TFT LCD

General Purpose Transistor PNP

Software & Tools

Arduino IDE

Project description

Code

Code without Graphical Touch-Ups

arduino

1/* ###############################################
2 I/O Assignments
3###############################################   */
4int _chSpeedSet    = A0,  // Speed setpoint
5    _chKp          = A1,  //   Proportional coefficient reading for PID controller
6    _chKi          = A2,   // Integral coefficient reading for PID controller
7    _chKd          = A3,   // Derivative coefficient reading for PID controller
8    _chMotorCmdCCW = 3,    // PWM output to motor for counter-clockwise turn
9    _chMotorCmdCW  = 2,    // PWM output to motor for clockwise turn
10    _chSpeedRead   = 24,  // Speed   reading
11    _chDirection   = 25;  // Direction selector reading
12
13/* ###############################################
14   Other Constants 
15############################################### */
16#define   _minRPM        0  // Minimum RPM to initiate direction changing
17#define _maxRPM      6000  // Maximum RPM limit
18#define _DiscSlots    20  // Qty of slots on   Index Disc
19
20/* ###############################################
21 Global   Variables
22############################################### */
23boolean Direction,   prevDirection;
24float RPM=0.0,     RPMset=0.0, OutputRPM=0.0,
25      Kp=0.0,     Ki=0.0,       Kd=0.0, 
26      Kpmax=2.0, Kimax=1.0,    Kdmax=1.0,
27      E=0.0,      Eprev=0.0,    dT=1.0;
28
29/* ###############################################
30   readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed)
31 Frequency Reading Function
32   Input Parameters:
33 (int)   _DI_FrequencyCounter_Pin : Digital pin to be read
34   (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1)
35
36   Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted.
37          It cannot be 0(zero), negative values or a value greater than 10.
38         When   _ReadingSpeed changed, 1 second shall be divided by this value to calculate
39          required counting duration. For example;
40          - _ReadingSpeed =   0.1 -> input shall be counted during 10 seconds (=1/0.1)
41          - _ReadingSpeed   = 0.5 -> input shall be counted during 2 seconds (=1/0.5)
42          - _ReadingSpeed   = 2.0 -> input shall be counted during 0.5 seconds (=1/2)
43          - _ReadingSpeed   = 4.0 -> input shall be counted during 0.25 seconds (=1/4)
44         Importantly   note that, increasing of _ReadingSpeed is a disadvantage especially
45         on   lower frequencies (generally below 100 Hz) since counting error increases 
46         up   to 20%~40% by decreasing frequency.
47###############################################   */
48
49int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
50{
51   pinMode(_DI_FrequencyCounter_Pin,INPUT);
52  byte _DigitalRead, _DigitalRead_Previous   = 0;
53  unsigned long _Time = 0, _Time_Init;
54  float _Frequency = 0;
55  if   ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
56     else
57       {
58          _Time_Init = micros();
59         do
60          {
61           _DigitalRead   = digitalRead(_DI_FrequencyCounter_Pin);
62           if ( (_DigitalRead_Previous==1)   && (_DigitalRead==0) ) _Frequency++;
63           _DigitalRead_Previous = _DigitalRead;
64            _Time = micros();
65          }
66         while ( _Time < (_Time_Init   + (1000000/_ReadingSpeed)) );
67       }
68  return (_ReadingSpeed * _Frequency);
69}
70/*   ########### End of readFrequency() ########### */
71/* ##############################################   */
72
73/* ###############################################
74 controllerPID(RangeMin,   RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd)
75 PID Controller Function
76 Input   Parameters:
77 (float) RangeMin: Minimum limit for output
78 (float) RangeMax:   Maximum limit for output
79 (float) _E_____: Current error signal
80 (float) _Eprev   : Previous error signal
81 (float) _dT____: Time difference as seconds
82 (float)   _Kp____: Proportional coefficient
83 (float) _Ki____: Integral coefficient
84   (float) _Kp____: Derivative coefficient
85 Adjustment procedure:
86   1. Set Kp=0,   Ki=0, Kd=0.
87   2. Start to increase Kp until the system oscillates at fixed period   (Pc) and note 
88      critical gain Kc = Kp.
89   3. Adjust final coefficients   as follows.
90      for P-control only  : Kp = 0.50*Kc
91      for PI-control   only : Kp = 0.45*Kc, Ki = 1.2/Pc
92      for PID-control     : Kp = 0.60*Kc, Ki   = 2.0/Pc, Kd=Pc/8
93   4. Fine tuning could be done by slightly changing each coefficient.
94###############################################   */
95 
96float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float   _Ki, float _Kd)
97{
98  float P, I, D;
99  /*
100   Base Formula: U = _Kp * (   _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
101   */
102  P = _Kp   * _E;                             /* Proportional Component */
103  I = _Kp * 0.5   * _Ki * (_E+_Eprev) * _dT;  /* Integral Component */
104  D = _Kp * _Kd * (_E-_Eprev)   / _dT;        /* Derivative Component */
105  return (P+I+D);
106}
107/* ###########   End of controllerPID() ########### */
108/* ##############################################   */
109
110/* ###############################################
111 Setup
112###############################################   */
113
114void setup()
115{
116  analogReadResolution(12);
117  pinMode(_chDirection,INPUT);     // Direction selector reading
118  pinMode(_chMotorCmdCCW,OUTPUT); // PWM output   to motor for counter-clockwise turn
119  pinMode(_chMotorCmdCW,OUTPUT);  // PWM   output to motor for clockwise turn
120  // Initial killing the PWM outputs to motor
121   analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
122  // Initial reading   for direction selection
123  Direction=digitalRead(_chDirection);   // HIGH=CCW,   LOW=CW
124  prevDirection=Direction;
125}
126
127
128/* ###############################################
129   Loop
130############################################### */
131
132void loop()
133{
134   // Initialization Time: Necessary for PID controller.
135        int InitTime   = micros();
136
137  // Reading Inputs
138     /* Controller Coefficients */
139         Kp = Kpmax * (float)analogRead(_chKp) / 4095;
140        Ki = Kimax * (float)analogRead(_chKi)   / 4095;
141        Kd = Kdmax * (float)analogRead(_chKd) / 4095;
142     /* Direction   Selector */   
143        Direction = digitalRead(_chDirection);  /* HIGH=CCW, LOW=CW   */ 
144     /* Actual RPM and RPM Setpoint 
145        Note that maximum selectable   RPM is 5000. */
146        RPM    = 60 * (float)readFrequency(_chSpeedRead,4) /   _DiscSlots;
147        RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095;
148
149   // Calculations and Actions
150     /* Error Signal, PID Controller Output and   Final Output (PWM) to Motor */
151        E = RPMset - RPM;
152        float cPID   = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
153        if ( RPMset == 0 ) OutputRPM   = 0;
154           else OutputRPM = OutputRPM + cPID; 
155        if ( OutputRPM   < _minRPM ) OutputRPM = _minRPM;
156        if ( OutputRPM > _maxRPM ) OutputRPM   = _maxRPM;
157                    
158     /* Changing Direction when inverted */
159         if ( Direction != prevDirection )
160          {
161            /* Killing   both of the PWM outputs to motor */
162               analogWrite(_chMotorCmdCCW,0);   analogWrite(_chMotorCmdCW,0);
163            /* Wait until motor speed decreases   */
164               do 
165                 { RPM = 60 * (float)readFrequency(_chSpeedRead,4)   / _DiscSlots; }
166               while ( RPM > _minRPM );
167          }
168  
169   // Writing Outputs
170        if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM));   
171                        else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM));
172
173   // Storing Values generated on previous cycle
174        Eprev = E; prevDirection   = Direction;
175
176  // Calculating control application cycle time and passed Seconds
177         dT = float ( micros() - InitTime ) / 1000000.0;
178}

/* ###############################################
 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;
}

Code with Graphical Touch-Ups

arduino

1/* ###############################################
2 Color constants  for Makeblock Me TFT LCD
3############################################### */
4#define  _BLACK  0
5#define _RED    1
6#define _GREEN  2
7#define _BLUE   3
8#define  _YELLOW 4
9#define _CYAN   5
10#define _PINK   6
11#define _WHITE  7
12
13/*  ###############################################
14 I/O Assignments
15###############################################  */
16int _chSpeedSet    = A0,  // Speed setpoint
17    _chKp          = A1,  //  Proportional coefficient reading for PID controller
18    _chKi          = A2,  // Integral coefficient reading for PID controller
19    _chKd          = A3,  // Derivative coefficient reading for PID controller
20    _chMotorCmdCCW = 3,   // PWM output to motor for counter-clockwise turn
21    _chMotorCmdCW  = 2,   // PWM output to motor for clockwise turn
22    _chSpeedRead   = 24,  // Speed  reading
23    _chDirection   = 25;  // Direction selector reading
24
25/* ###############################################
26  Other Constants 
27############################################### */
28#define  _minRPM        0  // Minimum RPM to initiate direction changing
29#define _maxRPM     6000  // Maximum RPM limit
30#define _Tmax         90  // Maximum time limit  for graphing
31#define _DiscSlots    20  // Qty of slots on Index Disc
32
33/*  ###############################################
34 Global Variables
35###############################################  */
36String  Cartesian_SetupDetails;
37boolean Direction, prevDirection;
38//  Alarm Settings
39float RALL=500.0, RAL=1000.0, RAH=4000.0, RAHH=4500.0;
40float  Seconds=0.0, prevSeconds=0.0, 
41      prevRPM=0.0, prevRPMset=0.0,
42      RPM=0.0,     RPMset=0.0, OutputRPM=0.0,
43      Kp=0.0,    Ki=0.0,       Kd=0.0, 
44      Kpmax=2.0,  Kimax=1.0,    Kdmax=1.0,
45      E=0.0,     Eprev=0.0,    dT=1.0;
46
47/* ###############################################
48  CommandToTFT(TFTCmd)
49 Command Function for Makeblock Me TFT LCD
50 Input Parameters:
51  (String) TFTCmd : Command string
52###############################################  */
53void CommandToTFT(String TFTCmd)
54{
55  /* Serial Connection used for display  */  
56  Serial1.println(TFTCmd); delay(5);
57}
58/* ########### End of CommandToTFT()  ########### */
59/* ############################################# */
60
61/*  ###############################################
62 Cartesian_Setup(Xmin, Xmax,  Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX,  ColorY)
63 Cartesian X-Y Axis Drawing Function for Makeblock Me TFT LCD
64 Input  Parameters:
65 (float) Xmin, Xmax, Ymin, Ymax : Axis range values
66 (int)   Window_X1,  Window_Y1___: Upper-left corner of graph window
67 (int)   Window_X2, Window_Y2___:  Lower-right corner of graph window
68 (int)   MinDashQty_____________: Qty.of dashes  on shortest axis
69 (int)   ColorB, ColorX, ColorY : Drawing colors for Frame,  X-axis and Y-axis
70 Uses external function CommandToTFT().
71###############################################  */
72String Cartesian_Setup(
73     float Xmin, float Xmax, float Ymin, float  Ymax, 
74     int Window_X1, int Window_Y1, int Window_X2, int Window_Y2, 
75     int MinDashQty, int ColorF, int ColorX, int ColorY
76     )
77{
78    /*  Screen Limitations */ 
79    const int DisplayResolutionX = 319, DisplayResolutionY  = 239;
80    /* Limit Title Strings */
81                  String XminTxt;
82                  if (abs(Xmin)>=1000000000)        XminTxt = "X=" + String (Xmin/1000000000)  + "G";
83                     else if (abs(Xmin)>=1000000)   XminTxt = "X="  + String (Xmin/1000000) + "M";
84                          else if (abs(Xmin)>=1000)  XminTxt = "X=" + String (Xmin/1000) + "K";
85                               else                 XminTxt = "X=" + String (Xmin);
86                  String XmaxTxt;
87                  if (abs(Xmax)>=1000000000)        XmaxTxt = "X=" + String (Xmax/1000000000)  + "G";
88                     else if (abs(Xmax)>=1000000)   XmaxTxt = "X="  + String (Xmax/1000000) + "M";
89                          else if (abs(Xmax)>=1000)  XmaxTxt = "X=" + String (Xmax/1000) + "K";
90                               else                 XmaxTxt = "X=" + String (Xmax);
91                  String YminTxt;
92                  if (abs(Ymin)>=1000000000)        YminTxt = "Y=" + String (Ymin/1000000000)  + "G";
93                     else if (abs(Ymin)>=1000000)   YminTxt = "Y="  + String (Ymin/1000000) + "M";
94                          else if (abs(Ymin)>=1000)  YminTxt = "Y=" + String (Ymin/1000) + "K";
95                               else                 YminTxt = "Y=" + String (Ymin);
96                  String YmaxTxt;
97                  if (abs(Ymax)>=1000000000)        YmaxTxt = "Y=" + String (Ymax/1000000000)  + "G";
98                     else if (abs(Ymax)>=1000000)   YmaxTxt = "Y="  + String (Ymax/1000000) + "M";
99                          else if (abs(Ymax)>=1000)  YmaxTxt = "Y=" + String (Ymax/1000) + "K";
100                               else                 YmaxTxt = "Y=" + String (Ymax);
101    /* Limits */  int XminPx  = Window_X1+1; int XmaxPx = Window_X2-1; 
102                  int YmaxPx = Window_Y1+1;  int YminPx = Window_Y2-1;
103    /* Origin */  int OriginX = XminPx + (int)( (XmaxPx  - XminPx) * abs(Xmin) / (abs(Xmax)+abs(Xmin)) );
104                  int OriginY  = YmaxPx + (int)( (YminPx - YmaxPx) * abs(Ymax) / (abs(Ymax)+abs(Ymin)) );
105    /*  Frame */   CommandToTFT ( "BOX(" + String(Window_X1) + "," + String(Window_Y1)+  "," + 
106                                          String(Window_X2) + ","  + String(Window_Y2)+ "," + 
107                                          String(ColorF)    + ");"
108                               );
109    /* X Axis */  CommandToTFT  ( "PL(" + String(Window_X1+1) + "," + String(OriginY) + "," + 
110                                         String(Window_X2-1)  + "," + String(OriginY) + "," + 
111                                         String(ColorX)            + ");" 
112                               );
113    /* Y Axis */  CommandToTFT  ( "PL(" + String(OriginX) + "," + String(Window_Y1+1) + "," + 
114                                         String(OriginX)  + "," + String(Window_Y2-1) + "," + 
115                                         String(ColorY)            + ");"
116                               );
117
118    /* 
119     Dashing:  Minimum amount of dashes is given by "MinDashQty" and will be dashed on the shortest  
120     axis-side with respect to origin.
121     On the other sections, dashes  to be marked shall be determined by considering ratio to 
122     shortest axis-side.
123     */
124    /* Dashing */  int XlengthLeft  = abs(XminPx-OriginX);   int XlengthRight  = abs(XmaxPx-OriginX);
125                   int YlengthLower = abs(YminPx-OriginY);   int YlengthUpper = abs(YmaxPx-OriginY);
126                   int XlengthLeft_Mod,  XlengthRight_Mod, YlengthLower_Mod, YlengthUpper_Mod;
127                   if (XlengthLeft<=1)  XlengthLeft_Mod=32767;  else XlengthLeft_Mod=XlengthLeft;
128                   if  (XlengthRight<=1) XlengthRight_Mod=32767; else XlengthRight_Mod=XlengthRight;
129                   if (YlengthLower<=1) YlengthLower_Mod=32767; else YlengthLower_Mod=YlengthLower;
130                   if (YlengthUpper<=1) YlengthUpper_Mod=32767; else YlengthUpper_Mod=YlengthUpper;
131                   int MinAxisLength = min ( min (XlengthLeft_Mod,XlengthRight_Mod),  min (YlengthLower_Mod,YlengthUpper_Mod) );
132                   int XdashesLeft  = MinDashQty * XlengthLeft / MinAxisLength;
133                   int XdashesRight  = MinDashQty * XlengthRight / MinAxisLength;
134                   int YdashesLower  = MinDashQty * YlengthLower / MinAxisLength;
135                   int YdashesUpper  = MinDashQty * YlengthUpper / MinAxisLength;
136                   int DashingInterval=2;  // Min.interval btw.dashes
137    
138    /* X-Dash L */  DashingInterval  = (int)  (XlengthLeft / XdashesLeft);
139                    if (!(DashingInterval<2))
140                       for (int i=OriginX; i>=XminPx; i-=DashingInterval)
141                          CommandToTFT  ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 
142                                                 String(i)  + "," + String(OriginY+2) + "," + 
143                                                 String(ColorX)  + ");"
144                                       );
145    /* X-Dash R */  DashingInterval  = (int) (XlengthRight / XdashesRight);
146                    if (!(DashingInterval<2))
147                       for (int i=OriginX; i<=XmaxPx; i+=DashingInterval)
148                           CommandToTFT  ( "PL(" + String(i) + "," + String(OriginY-2) + "," + 
149                                                  String(i)  + "," + String(OriginY+2) + "," + 
150                                                  String(ColorX)  + ");"
151                                        );
152    /* Y-Dash-L */  DashingInterval  = (int) (YlengthLower / YdashesLower);
153                    if (!(DashingInterval<2))
154                       for (int i=OriginY; i<=YminPx; i+=DashingInterval)
155                           CommandToTFT  ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 
156                                                  String(OriginX+2)  + "," + String(i) + "," + 
157                                                  String(ColorY)  + ");"
158                                        );
159    /* Y-Dash-U */  DashingInterval  = (int) (YlengthUpper / YdashesUpper);
160                    if (!(DashingInterval<2))
161                       for (int i=OriginY; i>=YmaxPx; i-=DashingInterval)
162                           CommandToTFT  ( "PL(" + String(OriginX-2) + "," + String(i) + "," + 
163                                                  String(OriginX+2)  + "," + String(i) + "," + 
164                                                  String(ColorY)  + ");"
165                                        );
166
167    /* Calculating  coordinates to display axis endpoint values */
168    int XminTxtX = Window_X1 -  (int)(XminTxt.length()*6) - 1,
169        XminTxtY = OriginY, 
170        XmaxTxtX  = Window_X2 + 1,
171        XmaxTxtY = OriginY, 
172        YminTxtX = OriginX,
173        YminTxtY = Window_Y2 + 1,
174        YmaxTxtX = OriginX,
175        YmaxTxtY  = Window_Y1 - 12 - 1;
176    /*  Controls: If any coordinate is -1, it shall fall  beyond display limits 
177        and respective value shall not be displayed */
178    if (XminTxtX<0) XminTxtX = -1;
179    if ( (XminTxtY-12) < 0 ) XminTxtY = -1;
180    if ( (XmaxTxtX+6*XmaxTxt.length()) > DisplayResolutionX ) XmaxTxtX = -1;
181    if ( (XmaxTxtY+12) > DisplayResolutionY ) XmaxTxtY = -1;
182    if ( (YminTxtX+6*YminTxt.length())  > DisplayResolutionX ) YminTxtX = -1;
183    if ( (YminTxtY+12) > DisplayResolutionY  ) YminTxtY = -1;
184    if ( (YmaxTxtX+6*YmaxTxt.length()) > DisplayResolutionX  ) YmaxTxtX = -1;
185    if (YmaxTxtY<0) YmaxTxtY = -1;
186
187     /* Range Limit  Titles */
188     if ( ( XminTxtX != -1 ) && ( XminTxtY != -1 ) )
189        CommandToTFT(  "DS12(" + String(XminTxtX) + "," + String(XminTxtY) + ",'" + String(XminTxt)  + "'," + String(ColorX) + ");" );
190     if ( ( XmaxTxtX != -1 ) && ( XmaxTxtY  != -1 ) )
191        CommandToTFT( "DS12(" + String(XmaxTxtX) + "," + String(XmaxTxtY)  + ",'" + String(XmaxTxt) + "'," + String(ColorX) + ");" );
192     if ( (  YminTxtX != -1 ) && ( YminTxtY != -1 ) )
193        CommandToTFT( "DS12(" + String(YminTxtX)  + "," + String(YminTxtY) + ",'" + String(YminTxt) + "'," + String(ColorY)  + ");" );
194     if ( ( YmaxTxtX != -1 ) && ( YmaxTxtY != -1 ) )
195        CommandToTFT(  "DS12(" + String(YmaxTxtX) + "," + String(YmaxTxtY) + ",'" + String(YmaxTxt)  + "'," + String(ColorY) + ");" );
196
197    /* 
198       Return Value String
199       Cartesian_Setup() will return a string packing graphic configuration in following  format:
200       "<Xmin,Xmax,Ymin,Ymax,Window_X1,Window_Y1,Window_X2,Window_Y2>"
201       String starts with '<' and ends by '>'. Each value is delimited by ','  
202    */ 
203    /* Initialize    */ String Cartesian_SetupDetails = "<";
204                        Cartesian_SetupDetails  += ( String(Xmin) + "," );
205                        Cartesian_SetupDetails +=  ( String(Xmax) + "," );
206                        Cartesian_SetupDetails += (  String(Ymin) + "," );
207                        Cartesian_SetupDetails += ( String(Ymax)  + "," );
208                        Cartesian_SetupDetails += ( String(Window_X1)  + "," );
209                        Cartesian_SetupDetails += ( String(Window_Y1)  + "," );
210                        Cartesian_SetupDetails += ( String(Window_X2)  + "," );
211                        Cartesian_SetupDetails += ( String(Window_Y2)  + "," );
212    /* Close-Out     */ Cartesian_SetupDetails += ">";
213
214return  Cartesian_SetupDetails;
215}
216/* ########### End of Cartesian_Setup() ###########  */
217/* ################################################ */
218
219/* ###############################################
220  Cartesian_ClearPlotAreas(Descriptor, Color)
221 Plot Area Reset/Clear Function for  Makeblock Me TFT LCD
222 Input Parameters:
223 (String) Descriptor : Setup Descriptor  - returned by Cartesian_Setup()
224 (int)    Color______: Color to be used to fill  plot area
225 Uses external function CommandToTFT().
226###############################################  */
227void Cartesian_ClearPlotAreas(String Descriptor, int Color)
228{
229  int  X1,Y1,X2,Y2;      /* Boundary coordinates for plot areas */
230  /* Extracting values  from Descriptor */
231  /* L[0] L[1] L[2] L[3] W[0]      W[1]      W[2]      W[3]       */
232  /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2  */
233  float L[4]; int W[4]; /* Values stored in Descriptor */
234  int j=0;              /*  Counter */
235  String D_Str = "";
236  for (int i=1; i<=(Descriptor.length()-1);  i++)
237      if ( Descriptor[i] == ',' )
238         { 
239           if (j<4)  L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt();
240           D_Str=""; j++;  
241         }
242         else
243         D_Str += Descriptor[i];
244
245  /*  Origin */  int OriginX = (W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) /  (abs(L[1])+abs(L[0])) );
246                int OriginY = (W[1]+1) + (int)( ( (W[3]-1)  - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2])) );
247
248  /* Clearing Plot Areas  */  
249  //Area.1 : X+ Y+
250  X1 = OriginX + 2 ; Y1 = W[1] + 1 ;
251  X2 = W[2]  - 1    ; Y2 = OriginY - 2 ;
252  CommandToTFT ( "BOXF(" + String(X1) + "," +  String(Y1) + "," + 
253                           String(X2) + "," + String(Y2)  + "," +
254                           String(Color)     + ");"
255               );
256  //Area.2 : X- Y+
257  X1 = W[0] + 1 ;    Y1 = W[1] + 1 ;
258  X2 = OriginX - 2  ; Y2 = OriginY - 2 ;
259  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1)  + "," + 
260                           String(X2) + "," + String(Y2) + ","  +
261                           String(Color)     + ");"
262               );
263  //Area.3 : X- Y-
264  X1 = W[0] + 1 ;    Y1 = OriginY + 2 ;
265  X2 = OriginX  - 2 ; Y2 = W[3] - 1 ;
266  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1)  + "," + 
267                           String(X2) + "," + String(Y2) + ","  +
268                           String(Color)     + ");"
269               );
270  //Area.4 : X+ Y-
271  X1 = OriginX + 2 ; Y1 = OriginY + 2 ;
272  X2 = W[2] - 1  ;    Y2 = W[3] - 1 ;
273  CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1)  + "," + 
274                           String(X2) + "," + String(Y2) + ","  +
275                           String(Color)     + ");"
276               );
277}  
278/* ########### End of Cartesian_ClearPlotAreas() ########### */
279/* #########################################################  */
280
281/* ###############################################
282 Cartesian_Line(Xp,  Yp, X, Y, Descriptor, Color)
283 Cartesian Line Function for Makeblock Me TFT LCD
284  Input Parameters:
285 (int)    Xp, Yp_____: Previous plot coordinates - y value  vs x
286 (int)    X, Y_______: Current plot coordinates - y value vs x
287 (String)  Descriptor : Setup Descriptor - returned by Cartesian_Setup()
288 (int)    Color______:  Marking color to be used on (x,y)
289 Uses external function CommandToTFT().
290###############################################  */
291void Cartesian_Line(float Xp, float Yp, float X, float Y, String Descriptor,  int Color)
292{
293  /* Extracting values from Descriptor */
294  /* L[0] L[1] L[2]  L[3] W[0]      W[1]      W[2]      W[3]       */
295  /* Xmin Xmax Ymin Ymax Window_X1  Window_Y1 Window_X2 Window_Y2  */
296  float L[4]; int W[4]; /* Values stored in  Descriptor */
297  int j=0;              /* Counter */
298  String D_Str = "";
299  for (int i=1; i<=(Descriptor.length()-1); i++)
300      if ( Descriptor[i] ==  ',' )
301         { 
302           if (j<4) L[j]=D_Str.toFloat(); else W[j-4]=D_Str.toInt();
303           D_Str=""; j++; 
304         }
305         else
306         D_Str +=  Descriptor[i];
307
308  /* Origin */  int OriginX = (W[0]+1) + (int)( ( (W[2]-1)  - (W[0]+1) ) * abs(L[0]) / (abs(L[1])+abs(L[0])) );
309                int OriginY  = (W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3])+abs(L[2]))  );
310
311  int XminPx = W[0] + 1;
312  int XmaxPx = W[2] - 1;
313  int YmaxPx =  W[1] + 1;
314  int YminPx = W[3] - 1;
315  
316  if (Y>L[3]) Y=L[3];
317  if (Y<L[2])  Y=L[2];
318    
319  float RatioX = (float)( XmaxPx - XminPx ) / ( L[1] - L[0] );
320  float RatioY = (float)abs( YmaxPx - YminPx ) / ( L[3] - L[2] );
321
322  int DispXp  = XminPx + (int)( RatioX * ( Xp - L[0] ) );
323  int DispYp = YminPx - (int)( RatioY  * ( Yp - L[2] ) );
324  int DispX = XminPx + (int)( RatioX * ( X - L[0] ) );
325  int DispY = YminPx - (int)( RatioY * ( Y - L[2] ) );
326  if (!(
327      ( (  DispXp >= (OriginX-2) ) && ( DispXp <= (OriginX+2) ) ) ||
328      ( ( DispYp >=  (OriginY-2) ) && ( DispYp <= (OriginY+2) ) ) ||
329      ( ( DispX >= (OriginX-2)  ) && ( DispX <= (OriginX+2) ) ) ||
330      ( ( DispY >= (OriginY-2) ) && ( DispY  <= (OriginY+2) ) )
331      ))
332       CommandToTFT( "PL(" + String(DispXp)  + "," + String(DispYp) + "," + String(DispX) + "," + String(DispY) + ","  + String(Color) + ");" );
333}
334/* ########### End of Cartesian_Line() ###########  */
335/* ############################################### */
336
337/* ###############################################
338  readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed)
339 Frequency Reading Function
340  Input Parameters:
341 (int)   _DI_FrequencyCounter_Pin : Digital pin to be read
342  (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1)
343
344  Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted.
345         It cannot be 0(zero), negative values or a value greater than 10.
346         When  _ReadingSpeed changed, 1 second shall be divided by this value to calculate
347         required counting duration. For example;
348          - _ReadingSpeed =  0.1 -> input shall be counted during 10 seconds (=1/0.1)
349          - _ReadingSpeed  = 0.5 -> input shall be counted during 2 seconds (=1/0.5)
350          - _ReadingSpeed  = 2.0 -> input shall be counted during 0.5 seconds (=1/2)
351          - _ReadingSpeed  = 4.0 -> input shall be counted during 0.25 seconds (=1/4)
352         Importantly  note that, increasing of _ReadingSpeed is a disadvantage especially
353         on  lower frequencies (generally below 100 Hz) since counting error increases 
354         up  to 20%~40% by decreasing frequency.
355###############################################  */
356
357int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
358{
359  pinMode(_DI_FrequencyCounter_Pin,INPUT);
360  byte _DigitalRead, _DigitalRead_Previous  = 0;
361  unsigned long _Time = 0, _Time_Init;
362  float _Frequency = 0;
363  if  ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
364     else
365       {
366         _Time_Init = micros();
367         do
368          {
369           _DigitalRead  = digitalRead(_DI_FrequencyCounter_Pin);
370           if ( (_DigitalRead_Previous==1)  && (_DigitalRead==0) ) _Frequency++;
371           _DigitalRead_Previous = _DigitalRead;
372           _Time = micros();
373          }
374         while ( _Time < (_Time_Init  + (1000000/_ReadingSpeed)) );
375       }
376  return (_ReadingSpeed * _Frequency);
377}
378/*  ########### End of readFrequency() ########### */
379/* ##############################################  */
380
381/* ###############################################
382 controllerPID(RangeMin,  RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd)
383 PID Controller Function
384 Input  Parameters:
385 (float) RangeMin: Minimum limit for output
386 (float) RangeMax:  Maximum limit for output
387 (float) _E_____: Current error signal
388 (float) _Eprev  : Previous error signal
389 (float) _dT____: Time difference as seconds
390 (float)  _Kp____: Proportional coefficient
391 (float) _Ki____: Integral coefficient
392  (float) _Kp____: Derivative coefficient
393 Adjustment procedure:
394   1. Set Kp=0,  Ki=0, Kd=0.
395   2. Start to increase Kp until the system oscillates at fixed period  (Pc) and note 
396      critical gain Kc = Kp.
397   3. Adjust final coefficients  as follows.
398      for P-control only  : Kp = 0.50*Kc
399      for PI-control  only : Kp = 0.45*Kc, Ki = 1.2/Pc
400      for PID-control     : Kp = 0.60*Kc, Ki  = 2.0/Pc, Kd=Pc/8
401   4. Fine tuning could be done by slightly changing each coefficient.
402###############################################  */
403 
404float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float  _Ki, float _Kd)
405{
406  float P, I, D;
407  /*
408   Base Formula: U = _Kp * (  _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
409   */
410  P = _Kp  * _E;                             /* Proportional Component */
411  I = _Kp * 0.5  * _Ki * (_E+_Eprev) * _dT;  /* Integral Component */
412  D = _Kp * _Kd * (_E-_Eprev)  / _dT;        /* Derivative Component */
413  return (P+I+D);
414}
415/* ###########  End of controllerPID() ########### */
416/* ##############################################  */
417
418/* ###############################################
419 Setup
420###############################################  */
421
422void setup()
423{
424  Serial1.begin(9600);
425  Serial1.println("CLS(0);");delay(20);
426  analogReadResolution(12);
427
428  pinMode(_chDirection,INPUT);    // Direction  selector reading
429  pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for  counter-clockwise turn
430  pinMode(_chMotorCmdCW,OUTPUT);  // PWM output to motor  for clockwise turn
431
432  // Initial killing the PWM outputs to motor
433  analogWrite(_chMotorCmdCCW,0);  analogWrite(_chMotorCmdCW,0);
434      
435  // Initial reading for direction selection
436  Direction=digitalRead(_chDirection);   // HIGH=CCW, LOW=CW
437  prevDirection=Direction;
438  
439  // The section below prepares TFT LCD
440  // Cartesian_Setup(Xmin, Xmax,  Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX,  ColorY)
441  Cartesian_SetupDetails = Cartesian_Setup(0, _Tmax, _minRPM, _maxRPM,  20, 20, 220, 120, 10, 0, 7, 7);
442  CommandToTFT("DS12(250,10,'Dir: CW '," +  String(_WHITE) + ");");
443  CommandToTFT("DS12(250,25,'____ Set'," + String(_YELLOW)  + ");");
444  CommandToTFT("DS12(250,40,'____ RPM'," + String(_GREEN) + ");");
445  /* Alarm Values */
446  CommandToTFT("DS12(250,55,'AHH:" + String(RAHH) + "',"  + String(_WHITE) + ");");
447  CommandToTFT("DS12(250,70,'AH :" + String(RAH)  + "'," + String(_WHITE) + ");");
448  CommandToTFT("DS12(250,85,'AL :" +  String(RAL)  + "'," + String(_WHITE) + ");");
449  CommandToTFT("DS12(250,100,'ALL:"+  String(RALL) + "'," + String(_WHITE) + ");");
450  /* Alarm Window */
451  CommandToTFT("BOX(240,55,319,115,"  + String(_WHITE) + ");");
452  /* Alarm Lamps */
453  CommandToTFT("BOX(240,55,248,70,"  + String(_WHITE) + ");");
454  CommandToTFT("BOX(240,70,248,85," + String(_WHITE)  + ");");
455  CommandToTFT("BOX(240,85,248,100," + String(_WHITE) + ");");
456  CommandToTFT("BOX(240,100,248,115," + String(_WHITE) + ");");
457}
458
459
460/*  ###############################################
461 Loop
462###############################################  */
463
464void loop()
465{
466  
467  // Initialization Time: Necessary for PID  controller.
468        int InitTime = micros();
469        
470  // X-Axis Auto-Reset  for Graphing
471        if ( Seconds > 90.0 ) 
472          {
473            Seconds  = 0.0;
474            Cartesian_ClearPlotAreas(Cartesian_SetupDetails,0);
475          }
476          
477  // Reading Inputs
478     /* Controller Coefficients */
479        Kp  = Kpmax * (float)analogRead(_chKp) / 4095;
480        Ki = Kimax * (float)analogRead(_chKi)  / 4095;
481        Kd = Kdmax * (float)analogRead(_chKd) / 4095;
482     /* Direction  Selector */   
483        Direction = digitalRead(_chDirection);  /* HIGH=CCW, LOW=CW  */ 
484     /* Actual RPM and RPM Setpoint 
485        Note that maximum selectable  RPM is 5000. */
486        RPM    = 60 * (float)readFrequency(_chSpeedRead,4) /  _DiscSlots;
487        RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095;
488
489
490  // Calculations and Actions
491     /* Error Signal, PID Controller Output and  Final Output (PWM) to Motor */
492        E = RPMset - RPM;
493        float cPID  = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
494        if ( RPMset == 0 ) OutputRPM  = 0;
495           else OutputRPM = OutputRPM + cPID; 
496        if ( OutputRPM  < _minRPM ) OutputRPM = _minRPM;
497        if ( OutputRPM > _maxRPM ) OutputRPM  = _maxRPM;
498                    
499     /* Changing Direction when inverted 
500        Note that no any graphical indication is performed on this function.  */
501        if ( Direction != prevDirection )
502          {
503            /* Killing  both of the PWM outputs to motor */
504               analogWrite(_chMotorCmdCCW,0);  analogWrite(_chMotorCmdCW,0);
505            /* Wait until motor speed decreases  */
506               do 
507                 { RPM = 60 * (float)readFrequency(_chSpeedRead,4)  / _DiscSlots; }
508               while ( RPM > _minRPM );
509          }
510  
511  // Writing Outputs
512        if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM));  
513                        else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM));
514
515  // Graphing
516        /* Indicating Direction */
517           if (Direction==HIGH)  CommandToTFT("DS12(280,10,'CCW '," + String(_WHITE) + ");");
518                           else  CommandToTFT("DS12(280,10,'CW  '," + String(_WHITE) + ");");
519        /* Plotting  Curve */
520           Cartesian_Line(prevSeconds, prevRPMset, Seconds, RPMset,  Cartesian_SetupDetails, _YELLOW);
521           Cartesian_Line(prevSeconds, prevRPM,  Seconds, RPM, Cartesian_SetupDetails, _GREEN);
522        /* Indicating values of  RPM Setpoint, PID Controller Coefficients, 
523           Error Signal, PID Controller  Output and Final RPM Output (PWM) */
524           CommandToTFT( "DS12(20,150,'Set:  " + String(RPMset) + " rpm  " + 
525                                      "RPM:  " + String(RPM)    + " rpm  '," + String(_WHITE) + ");");
526           CommandToTFT(  "DS12(20,170,'Kp= " + String(Kp) + " " +
527                                      "Ki=  " + String(Ki) + " " +
528                                      "Kd= " + String(Kd)  + " " +
529                                      "dT= " + String(dT*1000) +  " ms   '," + String(_WHITE) + ");");
530           CommandToTFT( "DS12(20,190,'e=  "      + String(E)         + " " +
531                                      "cPID=  "   + String(cPID)      + " " +
532                                      "RPMout=  " + String(OutputRPM) + "         '," + String(_WHITE) + ");");
533        /*  Resetting Alarm Lamps */ 
534           CommandToTFT("BOXF(241,56,247,69," + String(_BLACK)  + ");");
535           CommandToTFT("BOXF(241,71,247,84," + String(_BLACK) +  ");");
536           CommandToTFT("BOXF(241,86,247,99," + String(_BLACK) + ");");
537           CommandToTFT("BOXF(241,101,247,114," + String(_BLACK) + ");");
538        /* Activating Necessary Alarm Lamps */ 
539           if (RPM>=RAHH)              CommandToTFT("BOXF(241,56,247,69,"  + String(_RED) + ");");
540           if ((RPM>=RAH)&&(RPM<RAHH)) CommandToTFT("BOXF(241,71,247,84,"  + String(_RED) + ");");
541           if ((RPM>RALL)&&(RPM<=RAL)) CommandToTFT("BOXF(241,86,247,99,"  + String(_RED) + ");");
542           if (RPM<=RALL)              CommandToTFT("BOXF(241,101,247,114,"  + String(_RED) + ");");
543
544  // Storing Values generated on previous cycle
545        Eprev = E; prevRPMset = RPMset;   prevRPM = RPM; 
546        prevSeconds  = Seconds; prevDirection = Direction;
547
548  // Calculating control application  cycle time and passed Seconds
549        dT = float ( micros() - InitTime ) / 1000000.0;
550        Seconds+=dT; 
551}

/* ###############################################
 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

1/* ###############################################
2 I/O Assignments
3###############################################  */
4int _chSpeedSet    = A0,  // Speed setpoint
5    _chKp          = A1,  //  Proportional coefficient reading for PID controller
6    _chKi          = A2,  // Integral coefficient reading for PID controller
7    _chKd          = A3,  // Derivative coefficient reading for PID controller
8    _chMotorCmdCCW = 3,   // PWM output to motor for counter-clockwise turn
9    _chMotorCmdCW  = 2,   // PWM output to motor for clockwise turn
10    _chSpeedRead   = 24,  // Speed  reading
11    _chDirection   = 25;  // Direction selector reading
12
13/* ###############################################
14  Other Constants 
15############################################### */
16#define  _minRPM        0  // Minimum RPM to initiate direction changing
17#define _maxRPM     6000  // Maximum RPM limit
18#define _DiscSlots    20  // Qty of slots on  Index Disc
19
20/* ###############################################
21 Global  Variables
22############################################### */
23boolean Direction,  prevDirection;
24float RPM=0.0,     RPMset=0.0, OutputRPM=0.0,
25      Kp=0.0,    Ki=0.0,       Kd=0.0, 
26      Kpmax=2.0, Kimax=1.0,    Kdmax=1.0,
27      E=0.0,     Eprev=0.0,    dT=1.0;
28
29/* ###############################################
30  readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed)
31 Frequency Reading Function
32  Input Parameters:
33 (int)   _DI_FrequencyCounter_Pin : Digital pin to be read
34  (float) _ReadingSpeed____________: Custom reading speed between 0...10 (Note.1)
35
36  Note.1: _ReadingSpeed is a value to specify how long shall the changes be counted.
37         It cannot be 0(zero), negative values or a value greater than 10.
38         When  _ReadingSpeed changed, 1 second shall be divided by this value to calculate
39         required counting duration. For example;
40          - _ReadingSpeed =  0.1 -> input shall be counted during 10 seconds (=1/0.1)
41          - _ReadingSpeed  = 0.5 -> input shall be counted during 2 seconds (=1/0.5)
42          - _ReadingSpeed  = 2.0 -> input shall be counted during 0.5 seconds (=1/2)
43          - _ReadingSpeed  = 4.0 -> input shall be counted during 0.25 seconds (=1/4)
44         Importantly  note that, increasing of _ReadingSpeed is a disadvantage especially
45         on  lower frequencies (generally below 100 Hz) since counting error increases 
46         up  to 20%~40% by decreasing frequency.
47###############################################  */
48
49int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed)
50{
51  pinMode(_DI_FrequencyCounter_Pin,INPUT);
52  byte _DigitalRead, _DigitalRead_Previous  = 0;
53  unsigned long _Time = 0, _Time_Init;
54  float _Frequency = 0;
55  if  ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1);
56     else
57       {
58         _Time_Init = micros();
59         do
60          {
61           _DigitalRead  = digitalRead(_DI_FrequencyCounter_Pin);
62           if ( (_DigitalRead_Previous==1)  && (_DigitalRead==0) ) _Frequency++;
63           _DigitalRead_Previous = _DigitalRead;
64           _Time = micros();
65          }
66         while ( _Time < (_Time_Init  + (1000000/_ReadingSpeed)) );
67       }
68  return (_ReadingSpeed * _Frequency);
69}
70/*  ########### End of readFrequency() ########### */
71/* ##############################################  */
72
73/* ###############################################
74 controllerPID(RangeMin,  RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd)
75 PID Controller Function
76 Input  Parameters:
77 (float) RangeMin: Minimum limit for output
78 (float) RangeMax:  Maximum limit for output
79 (float) _E_____: Current error signal
80 (float) _Eprev  : Previous error signal
81 (float) _dT____: Time difference as seconds
82 (float)  _Kp____: Proportional coefficient
83 (float) _Ki____: Integral coefficient
84  (float) _Kp____: Derivative coefficient
85 Adjustment procedure:
86   1. Set Kp=0,  Ki=0, Kd=0.
87   2. Start to increase Kp until the system oscillates at fixed period  (Pc) and note 
88      critical gain Kc = Kp.
89   3. Adjust final coefficients  as follows.
90      for P-control only  : Kp = 0.50*Kc
91      for PI-control  only : Kp = 0.45*Kc, Ki = 1.2/Pc
92      for PID-control     : Kp = 0.60*Kc, Ki  = 2.0/Pc, Kd=Pc/8
93   4. Fine tuning could be done by slightly changing each coefficient.
94###############################################  */
95 
96float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float  _Ki, float _Kd)
97{
98  float P, I, D;
99  /*
100   Base Formula: U = _Kp * (  _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT );
101   */
102  P = _Kp  * _E;                             /* Proportional Component */
103  I = _Kp * 0.5  * _Ki * (_E+_Eprev) * _dT;  /* Integral Component */
104  D = _Kp * _Kd * (_E-_Eprev)  / _dT;        /* Derivative Component */
105  return (P+I+D);
106}
107/* ###########  End of controllerPID() ########### */
108/* ##############################################  */
109
110/* ###############################################
111 Setup
112###############################################  */
113
114void setup()
115{
116  analogReadResolution(12);
117  pinMode(_chDirection,INPUT);    // Direction selector reading
118  pinMode(_chMotorCmdCCW,OUTPUT); // PWM output  to motor for counter-clockwise turn
119  pinMode(_chMotorCmdCW,OUTPUT);  // PWM  output to motor for clockwise turn
120  // Initial killing the PWM outputs to motor
121  analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0);
122  // Initial reading  for direction selection
123  Direction=digitalRead(_chDirection);   // HIGH=CCW,  LOW=CW
124  prevDirection=Direction;
125}
126
127
128/* ###############################################
129  Loop
130############################################### */
131
132void loop()
133{
134  // Initialization Time: Necessary for PID controller.
135        int InitTime  = micros();
136
137  // Reading Inputs
138     /* Controller Coefficients */
139        Kp = Kpmax * (float)analogRead(_chKp) / 4095;
140        Ki = Kimax * (float)analogRead(_chKi)  / 4095;
141        Kd = Kdmax * (float)analogRead(_chKd) / 4095;
142     /* Direction  Selector */   
143        Direction = digitalRead(_chDirection);  /* HIGH=CCW, LOW=CW  */ 
144     /* Actual RPM and RPM Setpoint 
145        Note that maximum selectable  RPM is 5000. */
146        RPM    = 60 * (float)readFrequency(_chSpeedRead,4) /  _DiscSlots;
147        RPMset = 5000 * (float)analogRead(_chSpeedSet) / 4095;
148
149  // Calculations and Actions
150     /* Error Signal, PID Controller Output and  Final Output (PWM) to Motor */
151        E = RPMset - RPM;
152        float cPID  = controllerPID(E, Eprev, dT, Kp, Ki, Kd);
153        if ( RPMset == 0 ) OutputRPM  = 0;
154           else OutputRPM = OutputRPM + cPID; 
155        if ( OutputRPM  < _minRPM ) OutputRPM = _minRPM;
156        if ( OutputRPM > _maxRPM ) OutputRPM  = _maxRPM;
157                    
158     /* Changing Direction when inverted */
159        if ( Direction != prevDirection )
160          {
161            /* Killing  both of the PWM outputs to motor */
162               analogWrite(_chMotorCmdCCW,0);  analogWrite(_chMotorCmdCW,0);
163            /* Wait until motor speed decreases  */
164               do 
165                 { RPM = 60 * (float)readFrequency(_chSpeedRead,4)  / _DiscSlots; }
166               while ( RPM > _minRPM );
167          }
168  
169  // Writing Outputs
170        if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM));  
171                        else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM));
172
173  // Storing Values generated on previous cycle
174        Eprev = E; prevDirection  = Direction;
175
176  // Calculating control application cycle time and passed Seconds
177        dT = float ( micros() - InitTime ) / 1000000.0;
178}

/* ###############################################
 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;
}

Downloadable files

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.

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.

Schematic for Control of Direction and Speed of a DC Motor

Comments

Only logged in users can leave comments

tolgadurudogan

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content