E38S6G5 and KY-040 Rotary Encoder with a BLDC motor interrupt problem

Hi all,

I'm just creating a project for a yarn winder based on this one:

I have added a rotary encoder E38S6G5 with a 80mm aluminium V pulley to be able to measure the yarn length more detailed. For that purpose I completely redesigned the 3D print to include a housing for the E38S6G5 (I call it "measurement encoder" for easier reading.. :slight_smile: )

To get input from the KY-040 encoder I created a RotaryEncoder class with header:

#ifndef RotaryEncoder_h
#define RotaryEncoder_h

#define R_START       0x0
#define R_CW_FINAL    0x1
#define R_CW_BEGIN    0x2
#define R_CW_NEXT     0x3
#define R_CCW_BEGIN   0x4
#define R_CCW_FINAL   0x5
#define R_CCW_NEXT    0x6
#define DIR_NONE      0x0
#define DIR_CW        0x10
#define DIR_CCW       0x20

#define Mode_Step   0
#define Mode_Length 1
#define Mode_Run    2
#define Mode_Ready  3

#include "Arduino.h"

class RotaryEncoder
{
  public:
    RotaryEncoder(int pinDT
                 ,int pinCLK
                 ,int pinSW
                 ,int minCounter
                 ,int maxCounter
                 ,unsigned long waitTimeLongPressed
                 );

    void ChangeCounter(int &Counter, int &inMode);     // change the variables of the calling procedure;
    bool SwitchPressed;
    bool SwitchDown;
    bool SwitchUp;
    bool HasCounterChanged;
    bool WasLongPressed;
    int  Direction;  // -1 CounterClockwise, 1 Clockwise    
    unsigned long WaitTimeLongPressed;
    unsigned long TimeSwitchDown;
    
  private:
    //static const unsigned char ttable[7][4];
    int prv_pinDT;
    int prv_pinCLK;    
    int prv_pinSW;
    int prv_minCounter;
    int prv_maxCounter;
    int prv_pinState;
    byte prv_lastSwitchState;
    unsigned long prv_lastPressTime;
    bool prv_singlePressDetected;
};

#endif

and code:

#include "Arduino.h"
#include "RotaryEncoder.h"

// Initialize the static constant table
static const unsigned char ttable[7][4] = 
{
  // R_START
  {R_START,    R_CW_BEGIN,  R_CCW_BEGIN, R_START},
  // R_CW_FINAL
  {R_CW_NEXT,  R_START,     R_CW_FINAL,  R_START | DIR_CW},
  // R_CW_BEGIN
  {R_CW_NEXT,  R_CW_BEGIN,  R_START,     R_START},
  // R_CW_NEXT
  {R_CW_NEXT,  R_CW_BEGIN,  R_CW_FINAL,  R_START},
  // R_CCW_BEGIN
  {R_CCW_NEXT, R_START,     R_CCW_BEGIN, R_START},
  // R_CCW_FINAL
  {R_CCW_NEXT, R_CCW_FINAL, R_START,     R_START | DIR_CCW},
  // R_CCW_NEXT
  {R_CCW_NEXT, R_CCW_FINAL, R_CCW_BEGIN, R_START},
};

RotaryEncoder::RotaryEncoder(int pinDT
                            ,int pinCLK
                            ,int pinSW
                            ,int minCounter
                            ,int maxCounter
                            ,unsigned long waitTimeLongPressed
                            )
              : SwitchPressed(false)            // all after ":" initializes the class members with the parameter values
               ,SwitchDown(false)
               ,SwitchUp(false)
               ,HasCounterChanged(false)
               ,WasLongPressed(false)
               ,WaitTimeLongPressed(waitTimeLongPressed)
               ,prv_pinDT(pinDT)            
               ,prv_pinCLK(pinCLK)
               ,prv_pinSW(pinSW)
               ,prv_minCounter(minCounter)
               ,prv_maxCounter(maxCounter)
               ,prv_pinState(R_START)
               ,prv_lastSwitchState(HIGH)
               ,prv_lastPressTime(0)
               ,prv_singlePressDetected(false)
{
  pinMode(prv_pinDT, INPUT_PULLUP);
  pinMode(prv_pinCLK, INPUT_PULLUP);
  pinMode(prv_pinSW, INPUT_PULLUP);

  this->TimeSwitchDown = 0;

  //Serial.println("Initialized");
}

void RotaryEncoder::ChangeCounter(int  &Counter, int &inMode)     // change the variables of the calling procedure;
{  
  // Grab state of input pins.
  unsigned char pinState = (digitalRead(prv_pinCLK) << 1) | digitalRead(prv_pinDT);

  // Determine new state from the pins and state table.
  prv_pinState = ttable[prv_pinState & 0xf][pinState];

  // Clockwise rotation.
  if(prv_pinState == DIR_CW)
  {
    this->Direction = 1;
    if(Counter < prv_maxCounter)
    {
      Counter++;
      if(Counter > prv_maxCounter) Counter = prv_maxCounter;

      this->HasCounterChanged = true;
    }
  }

  // Counter clockwise rotation.
  else if(prv_pinState == DIR_CCW)
  {
    this->Direction = -1;

    if(Counter > prv_minCounter)
    {
      Counter--;
      if(Counter < prv_minCounter) Counter = prv_minCounter;

      this->HasCounterChanged = true;
    }
  }

  // Read the button state
  byte currentSwitchState = digitalRead(prv_pinSW);

  if (currentSwitchState == prv_lastSwitchState)
  {
      if (prv_lastPressTime > 0 && currentSwitchState == LOW)
      {
        unsigned long currentTime = millis();
        this->TimeSwitchDown = currentTime - prv_lastPressTime;
        //Serial.print(currentTime); Serial.print("\t"); Serial.println(this->TimeSwitchDown);
        
        if (this->TimeSwitchDown > this->WaitTimeLongPressed)
        {
          this->WasLongPressed = true;
        }
      }
  }
  else 
  {
    if(currentSwitchState == LOW)
    {
      // Button is pressed
      //Serial.println("Button pressed!");
      this->SwitchPressed = false;      // get true when released
      this->SwitchDown = true;
      this->SwitchUp = false;

      if (prv_lastPressTime == 0)
      {
        prv_lastPressTime = millis();
      }
    }
    else
    {
      // Button is released
      //Serial.print("Button released! \t");

      this->SwitchPressed = true;
      this->SwitchDown = false;
      this->SwitchUp = true;
      this->TimeSwitchDown = 0;
      this->WasLongPressed = false;
      prv_lastPressTime = 0;

      switch (inMode)
      {
        case Mode_Step:
        {
          inMode = Mode_Length;
          break;
        }
        case Mode_Length:
        {
          inMode = Mode_Step;
          break;
        }
      }
    }
    prv_lastSwitchState = currentSwitchState;
  }
}

(parts of the code are from here:
KY-040 rotary encoder skipping steps - Arduino Stack Exchange)

When I instantiate the class in the main code with:

RotaryEncoder    encoderStep  (pinKY40_DT, pinKY40_CLK, pinKY40_SW, 0, MAXSTEPVALUES - 1, WaitTime);
RotaryEncoder    encoderLength(pinKY40_DT, pinKY40_CLK, pinKY40_SW, 1, 10               , WaitTime);
RotaryEncoder    encoderSpeed (pinKY40_DT, pinKY40_CLK, pinKY40_SW, 0, 127              , WaitTime);

it works perfectly in the Loop procedure. I can use the "Step" part to enter a step value, switch with the button to "Length" mode and enter the yarn length. Next click switches back to Step and so on.
The class also gives information back when I press the switch for a longer time, which displays a progress bar in the display. If the desired time is reached, the loop switches to Run mode, the button is then only used to set the motor speed to 0, the KY-040 is used to change the speed of the motor.
Everything great until here.

Now the measurement encoder comes. I've added two interrupts in the setup:

  pinMode(pinENCODER_PIN_A, INPUT_PULLUP);
  pinMode(pinENCODER_PIN_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_A), readEncoder1, RISING);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_B), readEncoder2, RISING);

with the interrupt procedures:

void readEncoder1()
{
  if(inMode == Mode_Run)
  {
    // readEncoder1 is activated if DigitalPin nr 2 is going from LOW to HIGH
    // Check pin 3 to determine the direction
    if(digitalRead(pinENCODER_PIN_B)==LOW) 
    {
      yarnLengthMM += LENGTH_PER_PULSE_MM;
    }
    else
    {
      yarnLengthMM -= LENGTH_PER_PULSE_MM;
    }
  }
}

void readEncoder2()
{
  if(inMode == Mode_Run)
  {
    // readEncoder2 is activated if DigitalPin nr 3 is going from LOW to HIGH
    // Check pin 2 to determine the direction
    if(digitalRead(pinENCODER_PIN_A)==LOW) 
    {
      yarnLengthMM -= LENGTH_PER_PULSE_MM;
    }
    else
    {
      yarnLengthMM += LENGTH_PER_PULSE_MM;
    }
  }
}

In the loop procedure I can now turn the disc of the measurement encoder and it does what it should do: The measured length is displayed. Everything is good. But: When I want to change the motor speed with the KY-040 it turns out that the interrupts (600 pulses per 360°, so a lot more calls if the wheel turns faster) being called so often that the loop doesn't handle the KY-040 anymore so that I cannot (or as good as not) change the motor speed with the KY-040. When I stop the wheel of the measurement encoder to zero, I can handle the KY-040. But that would be no option as the motor is pulling the yarn over the wheel and would still do if I hold the wheel so a lot of measurement would be lost.

I've tried a lot of different possibilities like trying to read the KY-040 in the ISR or deactivating the interrupt and reading the measurement encoder in the loop (which is far too slow) or creating a state machine to switch between reading the measurement encoder and reading the KY-040 and also displaying something on the display.

What I found out so far: When the wheel is turned manually (interrupts happens) it display the progress as value and as progress bar without any delay. That means, the interrupt doesn't disturb the calculation of the values or displaying the values which is done in the loop in run mode. Means to me, the loop is fast enough to do all this. But it just don't handle the KY-040 with the "ChangeCounter" anymore until I stop the wheel. If I try to add the ChangeCounter call into the ISR (which is of course too slow for an ISR) the counter doesn't change at all (would of course only while I turning the wheel).

So I can only guess that reading the interrupts stops the Arduino (a Nano) from reading the pins for the KY-040 - if the loop were simply too slow it should also not handle the display anymore. Of course that cannot be seen directly as the display holds what it displays also if nothing is changed. But the values of the measurement encoder are displayed without delay so what could be the reason that the values of the KY-040 doesn't reach the code anymore?

It has also nothing to do with the BLDC motor, when I disable the code lines that change the motor speed the behaviour is the same.

I have also tried adding a TimerOne library with a Timer interrupt to handle the KY-040 which also didn't work.

I read somewhere that someone mentioned a user interrupt assignable to other pins than 2/3 which are reserved for the measurement encoder. It would be great to have the possibility to have an additional interrupt for the KY-040 so that both rotary encoders can be handled. It would mean that when the KY-040 is turned, maybe some interrupts of the measurement encoder would be lost, but I could live with that.

I cannot use another Arduino as I do not have any other and it is no room in the device to add a bigger one or one with another form factor. I can also rather not add any additional hardware into it.

I'm a bit lost now. Would be great if anyone has any idea.

Cheers,

Christian

Not shown is how yarnLengthMM is declared or used.  What type is it?  Have you added the volatile keyword?

It's generally better to post the entire code at once rather than selected snippets.  Gives context.  Still, not a bad first post.

What board is used?

This library can handle two encoders with each having only one interrupt capable pin.   It may work for you.

rotary encoder E38S6G5

Do you require that much resolution on the yarn length?

 attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_A), readEncoder1, RISING);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_B), readEncoder2, RISING);

Perhaps you could only read only the A or B channel and cut the number of interrupts in half.

Perhaps you could use one of the E38S6G5 models with fewer pulses per revolution. There appears to be models down to 100 pulses per revolution.

Hello,

thanks for your answers, in the meantime I found a solution which works without problems.

I will copy the resulting code of the main file below.

To answer your questions first, yes, yarnLengthMM was declared volatile and as float.

The board is an Arduino Nano (mentioned above but maybe you overread it).

Using only one interrupt is maybe a solution, so thanks for the suggestion, I didn't knew that this would be possible.

600p/360°: No, I would be satisfied with lower resolution, I only found this model first, didn't knew that there are others with different number of pulses. But it doesn't help now anymore as I already have this one now... :slight_smile:

I simply followed the instructions on how to connect this device to 2/3 pins for interrupt and I don't know if it works with one of these only. Would be another possibility.

But finally, after copy/pasting my own posting above to ChatGPT, it gave me the idea of how to solve it. There is a library called "PinChangeInterrupt" which can be downloaded from the Arduino IDE. Using this I can attach interrupts to other digital pins on changing signal. And this one works perfectly using these lines for the measurement encoder with the 2/3 hardware interrupt:

  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_A), readEncoder1, RISING);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_B), readEncoder2, RISING);

and additionally using these lines to attach the PinChangeInterrupt to the other 3 pins of the KY-040:

  attachPCINT(digitalPinToPCINT(pinKY40_DT) , handleKY040, CHANGE);
  attachPCINT(digitalPinToPCINT(pinKY40_CLK), handleKY040, CHANGE);
  attachPCINT(digitalPinToPCINT(pinKY40_SW) , handleKY040, CHANGE);

I could have done that also in the Setup procedure but this disturbs the reading of the pins in the loop in mode Length and mode Step, as there is already code to read out the pins. I didn't want to change that, so I attached these interrupts when switching to Run mode where it is needed to use an interrupt to be fast enough.

Still I do not really understand why the really fast Loop procedure is not able to read the pins for the KY-040 while at the same time the measurement encoder creates hardware interrupts - because the loop is fast enough to create all the display jobs which is in my eyes a lot more code lines to execute.

But now it works with the new interrupt routines.

Nevertheless, thanks for your suggestions!

Cheers,

Christian

Here is the final code for anyone who maybe get the same problem:

// Christian Coppes - WoolWinderPlus4
// Date: 01.10.2024

// Code for KY-040 from here: https://arduino.stackexchange.com/questions/69307/ky-040-rotary-encoder-skipping-steps
// Code for Rotary Encoder E38S6G5-600B-G24N from here: https://electricdiylab.com/how-to-connect-optical-rotary-encoder-with-arduino/
// Pins described here: https://sharvielectronics.com/product/e38s6g5-600b-g24n-photoelectric-incremental-rotary-encoder/
// Tip to use PinChangeInterrupt library is from ChatGPT, so Kudos for that tip to ChatGPT!
/*
Connection for E38S6G5-600B-G24N:

    Green = A phase
    White = B phase
    Red = +Vcc
    Black = GND
    Shield = The shield should be connected to GND to Reduce the Motor Noice.
*/

// KY-040 used for input of motor speed
// Rotary Encoder E38S6G5 is used for yarn measurement

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "RotaryEncoder.h"
#include <PinChangeInterrupt.h>

#define SCREEN_WIDTH    128   // OLED display width, in pixels
#define SCREEN_HEIGHT   64    // OLED display height, in pixels
#define SCREEN_ADDRESS  0x3C  // < See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
#define OLED_RESET      4
#define MAXSTEPVALUES   8     // number of step elements in the "stepvalues" array

#define Mode_Step   0         // step mode: enter the value to add or subtract to the yarn length in length mode
#define Mode_Length 1         // length mode: change the length of the complete yarn 
#define Mode_Run    2
#define Mode_Ready  3

const byte pinENCODER_PIN_A = 2;
const byte pinENCODER_PIN_B = 3;

const byte pinKY40_DT       = 9;  // Connected to DT on KY-040
const byte pinKY40_SW       = 8;  // Connected to SW on KY-040
const byte pinKY40_CLK      = 7;  // Connected to CLK on KY-040

const byte pinMotorSpeed     = 5;  // Motor speed control

const int MINSPEED              = 0;                                      // minimum speed possible to enter in setup
const int MAXSPEED              = 255;                                    // maximum speed possible to enter in setup
const float PULSES_PER_ROTATION = 600.0 * 2;                              // no. of pulses the E38S6G5 rotary encoder generates in 360°
const float DISC_RADIUS_MM      = 35.0;                                   // radius of the disc in mm on the yarn length rotary encoder
const float CIRCUMFERENCE_MM    = 2 * 3.14159265 * DISC_RADIUS_MM;        //as it is no 360° touching the wheel this cannot be used
const float CORRECTION_FACTOR   = 10.0 / 9.8;                             // measurement was 9.8m when 10m was entered in a test.
const float LENGTH_PER_PULSE_MM = (CIRCUMFERENCE_MM / PULSES_PER_ROTATION) * CORRECTION_FACTOR;
const float MAX_YARN_LENGTH     = 2000.0;
const unsigned long WaitTime    = 2000;                                   // time to wait if switch is pressed for a longer time, starts the run mode

// Declare the handleDisplay procedure
void handleDisplay();

int   inMode          = Mode_Step;    // 0 = Step, 1 = Length, 2 = Run
int   previousMode    = 0;
int   stepIndex       = 4;            // yarnlength initial step value =100 (see stepvalues array below)
int   dummyCounter    = 5;            // dummy counter which will not be used but needed for "ChangeCounter".
                                      // 5: ensure that it never reaches minimum (1) or maximum (10) so the rotary encoder can always be turned
int   speedCounter    = 0;            // motor speed
float yarnLength      = 500.0;        // initial length 500m
float progressBarWidth  = 0.0;

const float stepvalues[] =
{
  1.0,
  10.0,
  20.0,
  50.0,
  100.0,
  200.0,
  500.0,
  1000.0
};

volatile float yarnLengthMM = 0.0;
float previousYarnLengthMM = 0.0;

Adafruit_SSD1306 display      (SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
RotaryEncoder    encoderStep  (pinKY40_DT, pinKY40_CLK, pinKY40_SW, 0, MAXSTEPVALUES - 1, WaitTime);
RotaryEncoder    encoderLength(pinKY40_DT, pinKY40_CLK, pinKY40_SW, 1, 10               , WaitTime);
RotaryEncoder    encoderSpeed (pinKY40_DT, pinKY40_CLK, pinKY40_SW, 0, 127              , WaitTime);

//-------------------------------------------------------------------
void setup()
{
  pinMode(pinMotorSpeed, OUTPUT);

  // setup for measurement encoder E38S6G5
  pinMode(pinENCODER_PIN_A, INPUT_PULLUP);
  pinMode(pinENCODER_PIN_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_A), readEncoder1, RISING);
  attachInterrupt(digitalPinToInterrupt(pinENCODER_PIN_B), readEncoder2, RISING);

  // fast serial monitor setting adjust the serial monitor which uses 9600 by default
  Serial.begin(115200);
  delay(500);

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); // initialize with the I2C addr 0x3D (for the 128x64) (0x3C used for SD1315)

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,1);
  display.print("Woolwinder+");
  display.display();
  delay(1000);

  handleDisplay(); // show initial values
}

// As this part is needed in Length AND step mode it is an own procedure here.
// This switches from Length or step mode to run mode whenever the switch is pressed for several seconds.
// The number of 
void switchToRun()
{
  //Serial.println("Was LongPressed");
  inMode = Mode_Run;
  display.clearDisplay();
  display.display();
  handleDisplay();

  attachPCINT(digitalPinToPCINT(pinKY40_DT) , handleKY040, CHANGE);
  attachPCINT(digitalPinToPCINT(pinKY40_CLK), handleKY040, CHANGE);
  attachPCINT(digitalPinToPCINT(pinKY40_SW) , handleKY040, CHANGE);  
}

// -------------------------------------------------------------
void loop()
{
  switch (inMode) 
  {
    case Mode_Run:
    {
      // Handle KY-040 state changes
      if (encoderSpeed.HasCounterChanged || encoderSpeed.SwitchPressed) 
      {
        if (encoderSpeed.SwitchPressed) 
        {
          speedCounter = 0;
          analogWrite(pinMotorSpeed, speedCounter);
        }
        if (encoderSpeed.HasCounterChanged) 
        {
          analogWrite(pinMotorSpeed, speedCounter * 2);
        }
        encoderSpeed.HasCounterChanged = false;
        encoderSpeed.SwitchPressed = false;
        handleDisplay();
      }

      // handle the measurement encoder
      if (yarnLengthMM != previousYarnLengthMM)
      {
        previousYarnLengthMM = yarnLengthMM;

        if (previousYarnLengthMM / 1000.0 >= yarnLength) 
        {
          analogWrite(pinMotorSpeed, 0); // Stop the motor
          inMode = Mode_Ready;
        }

        if (previousYarnLengthMM < 0)
        {
          previousYarnLengthMM = 0;
          yarnLengthMM = 0;
        }

        handleDisplay();
      }

      break;
    }

    case Mode_Step:
    {
      encoderStep.ChangeCounter(stepIndex, inMode);
      if (encoderStep.SwitchDown)     {handleDisplay();}  // show the progress bar for viewing the time the switch is pressed

      if (encoderStep.WasLongPressed) {switchToRun();  }
      else 
      {      
        if (encoderStep.HasCounterChanged || encoderStep.SwitchPressed)
        {
          if(encoderStep.SwitchPressed)
          {
            encoderLength.HasCounterChanged = false;
            encoderLength.SwitchPressed = false;
          }
          
          encoderStep.HasCounterChanged = false;
          encoderStep.SwitchPressed = false;
          handleDisplay();
          previousMode = Mode_Step;
        }
      }
      break;
    }
    case Mode_Length:
    {
      // Only the rotary direction is needed here, so dummyCounter is used 
      encoderLength.ChangeCounter(dummyCounter, inMode);

      if (encoderLength.SwitchDown)     {handleDisplay();}  // show the progress bar for viewing the time the switch is pressed
      
      if (encoderLength.WasLongPressed) {switchToRun();  }
      else 
      {      
        if (encoderLength.HasCounterChanged || encoderLength.SwitchPressed)
        {
          if(encoderLength.SwitchPressed)
          {
            encoderStep.HasCounterChanged = false;
            encoderStep.SwitchPressed = false;
          }
          if(previousMode == Mode_Length)
          {
            yarnLength += stepvalues[stepIndex] * encoderLength.Direction;
            dummyCounter = 5; // ensure that it never reaches minimum (1) or maximum (10) so the rotary encoder can always be turned
            
            if (yarnLength > MAX_YARN_LENGTH) yarnLength = MAX_YARN_LENGTH;
            if (yarnLength < 0.0) yarnLength = 0.0;
          }

          encoderLength.HasCounterChanged = false;
          encoderLength.SwitchPressed = false;
          handleDisplay();
          previousMode = Mode_Length;
        }
      }
      break;
    }        
  }
}

void readEncoder1()
{
  if(inMode == Mode_Run)
  {
    // readEncoder1 is activated if DigitalPin nr 2 is going from LOW to HIGH
    // Check pin 3 to determine the direction
    if(digitalRead(pinENCODER_PIN_B)==LOW) {yarnLengthMM += LENGTH_PER_PULSE_MM;}
    else                                   {yarnLengthMM -= LENGTH_PER_PULSE_MM;}
  }
}

void readEncoder2()
{
  if(inMode == Mode_Run)
  {
    // readEncoder2 is activated if DigitalPin nr 3 is going from LOW to HIGH
    // Check pin 2 to determine the direction
    if(digitalRead(pinENCODER_PIN_A)==LOW) {yarnLengthMM -= LENGTH_PER_PULSE_MM;}
    else                                   {yarnLengthMM += LENGTH_PER_PULSE_MM;}
  }
}

void handleKY040()
{
  encoderSpeed.ChangeCounter(speedCounter, inMode);
}


//--------------------------------------------------------------
void handleDisplay() 
{
    switch(inMode)
    {
      case Mode_Step:
      {
        // Serial.println("Mode: Step");
        // Serial.print("StepIndex: \t")  ; Serial.print(stepIndex);   Serial.print("\t"); Serial.print("Step Value: \t") ; Serial.println(stepvalues[stepIndex]);
        // Serial.print("dummyCounter: \t"); Serial.print(dummyCounter); Serial.print("\t"); Serial.print("Yarn Length: \t"); Serial.println(yarnLength);
        // Serial.println("");

        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(WHITE);
        display.setCursor(0,1);
        display.print("Set L-Step: ");
        display.print(stepvalues[stepIndex], 0); // Display with 0 decimal place

        if (encoderStep.SwitchDown)
        {
          progressBarWidth = ((float)encoderStep.TimeSwitchDown / (float)WaitTime) * 128;
          display.drawRect(0, 48, 128, 16, WHITE);
          display.fillRect(0, 48, progressBarWidth, 16, WHITE);
        }
        display.display();
        break;
      }

      case Mode_Length:
      {
        // Serial.println("Mode: Length");
        // Serial.print("StepIndex: \t")  ; Serial.print(stepIndex);   Serial.print("\t"); Serial.print("Step Value: \t") ; Serial.println(stepvalues[stepIndex]);
        // Serial.print("dummyCounter: \t"); Serial.print(dummyCounter); Serial.print("\t"); Serial.print("Yarn Length: \t"); Serial.println(yarnLength);
        // Serial.println("");

        display.clearDisplay();
        display.setTextSize(2);
        display.setTextColor(WHITE);
        display.setCursor(0,1);
        display.print("Set Max L: ");
        display.print(yarnLength, 0); // Display with 0 decimal place
        display.print(" m");

        if (encoderLength.SwitchDown)
        {
          progressBarWidth = ((float)encoderLength.TimeSwitchDown / (float)WaitTime) * 128;
          display.drawRect(0, 48, 128, 16, WHITE);
          display.fillRect(0, 48, progressBarWidth, 16, WHITE);
        }
        display.display();

        break;
      }

      case Mode_Run:
      {
        // Serial.println("Run Mode"); Serial.print("speedCounter: \t"); Serial.print(speedCounter); Serial.println("\t");

        display.fillRect(0, 0, 128, 16, 0);
        display.setTextSize(2);
        display.setTextColor(WHITE);
        display.setCursor(0,1);
        display.print("Speed: ");
        display.println(speedCounter);
        display.print("ML:");
        display.print(yarnLength, 0); // Display with 0 decimal place
        display.print(" m");
        

        display.fillRect(0, 34, 128, 32, 0);      
        display.setTextSize(2);
        display.setCursor(0, 40);
        display.print("L:");
        display.print(previousYarnLengthMM / 1000.0, 2); // Display with 1 decimal places
        display.println("m");

        // Calculate and display the progress bar
        progressBarWidth = ((previousYarnLengthMM /1000.0) / yarnLength) * 128;
        display.fillRect(0, 55, 128, 9, 0);
        display.drawRect(0, 55, 128, 9, WHITE);
        display.fillRect(0, 55, progressBarWidth, 10, WHITE);

        display.display();

        // Serial.print("Yarn Length: ");  Serial.print(yarnLengthCopy / 1000.0); // Convert mm to meters
        // Serial.println(" m");
        //Serial.print(previousYarnLengthMM / 1000.0);
        break;
      }

      case Mode_Ready:
      {
        // Serial.println("Ready!!");
        display.clearDisplay();
        display.setTextSize(3);
        display.setCursor(5, 25);
        display.print("Ready!");
        display.display();
        break;
      }
    }
}