Need assistance with the loop function

Hello everyone, it is my first time posting in this forum.
Let me first state out the problem statement of the project.
There are two stepper motor, let's say, A and B. A is an independent motor equipped with AS5600 sensor with diametrically magnetized magnet with simple job of determining step and rpm of the motor A.
And then motor B is attached to the ball valve to stop the flow of the fluids. After certain minimal rpm range motor B should rotate clockwise or anticlockwise (doesn't matter).
So on to the code then, first to home and constraint motor, I have added to two limit switches UPPER_LIMIT and LOWER_LIMIT. Motor is controlled using Arduino Uno and A4988 motor driver and it is using Accelstepper library to do motion. The pins connections are very straight forward
And there is a temporary OLED display 1306 to monitor the sensor.
So, everything works including homing, data measuring and monitoring from AS5600 except loop function.
Could you be kind enough to tell me where I went wrong or where made a foolish mistake.
The main issue of the code mentioned above is after homing, the stepper doesn't move according to the loop function algorithm, it just stays at idle position.

#include "TimerOne.h"          //Timer interrupts
#include <Wire.h>              //This is for i2C
#include <SSD1306Ascii.h>      //i2C OLED
#include <SSD1306AsciiWire.h>  //i2C OLED
// i2C OLED
#define I2C_ADDRESS 0x3C
#define RST_PIN -1
#define LOWER_LIMIT 3
#define UPPER_LIMIT 4
#define EN 10
#define RST 11
#define STEP 7
#define DIR 6
#define INTERFACE_TYPE 1
#include <AccelStepper.h>
byte controlPins[] = { EN, RST, STEP, DIR };
long maxTravel = 200000;  // max distance you could be away from zero switch
long maxBackup = 200;     // max distance to correct limit switch overshoot

// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(INTERFACE_TYPE, STEP, DIR);

SSD1306AsciiWire oled;
float OLEDTimer = 0;  //Timer for the screen refresh

//---------------------------------------------------------------------------
//Magnetic sensor things
int magnetStatus = 0;  //value of the status register (MD, ML, MH)

int lowbyte;     //raw angle 7:0
word highbyte;   //raw angle 7:0 and 11:8
int rawAngle;    //final raw angle
float degAngle;  //raw angle in degrees (360/4096 * [value between 0-4095])

int quadrantNumber, previousquadrantNumber;  //quadrant IDs
float numberofTurns = 0;                     //number of turns
float correctedAngle = 0;                    //tared angle - based on the startup value
float startAngle = 0;                        //starting angle
float totalAngle = 0;                        //total absolute angular displacement
float previoustotalAngle = -1;               //for the display printing
float recentTotalAngle = 0;                  //for the display printing
float rpmValue = 0;                          //revolutions per minute
float stepRateFromRPM = 0;                   //steps/s calculated from RPM
float rpmInterval = 200;                     // RPM is calculated every 0.2 seconds
float rpmTimer = 0;                          //Timer for the rpm
float timerdiff = 0;                         //Time difference for more exact rpm calculation

void setup() {
  //This uses the timer1 of the Arduino
  Timer1.initialize(100);  //100 us timer trigger interval
  Timer1.setPeriod(100);
  //------------------------------------------------------------------------------

  Serial.begin(115200);    //start serial - tip: don't use serial if you don't need it (speed considerations)
  Wire.begin();            //start i2C
  Wire.setClock(800000L);  //fast clock

  checkMagnetPresence();  //check the magnet (blocks until magnet is found)

  ReadRawAngle();         //make a reading so the degAngle gets updated
  startAngle = degAngle;  //update startAngle with degAngle - for taring

  //------------------------------------------------------------------------------
  //OLED part
#if RST_PIN >= 0
  oled.begin(&Adafruit128x32, I2C_ADDRESS, RST_PIN);
#else   // RST_PIN >= 0
  oled.begin(&Adafruit128x32, I2C_ADDRESS);
#endif  // RST_PIN >= 0

  //------------------------------------------------------------------------------

  oled.setFont(Adafruit5x7);
  oled.clear();              //clear display
  oled.set2X();              //double-line font size - better to read it
  oled.println("  AS5600");  //print a welcome message
  oled.set1X();              //double-line font size - better to read it
  delay(500);
  OLEDTimer = millis();  //start the timer
  oled.clear();
  refreshDisplay();
  pinMode(LOWER_LIMIT, INPUT_PULLUP);
  pinMode(UPPER_LIMIT, INPUT_PULLUP);
  for (int i = 0; i < 4; i++) {
    pinMode(controlPins[i], OUTPUT);
    digitalWrite(controlPins[i], LOW);
  }

  delay(200);               // hold reset until we can see it on the scope
  digitalWrite(RST, HIGH);  // take out of reset
  delay(2);                 // allow charge pump on driver to start

  Serial.begin(115200);
  Serial.println("Stepper calibration");
  // Set the maximum speed and acceleration for calibration:
  stepper.setMaxSpeed(400);  // note 1000 is the fastest speed for this library
  stepper.setAcceleration(500);
  findEnd(LOWER_LIMIT, 1);  // hit the zero limit switch
  backup(LOWER_LIMIT, 0);   // nudge back so switch is not triggered
  // Set the maximum speed and acceleration for use:
  stepper.setCurrentPosition(0);  // set this point as zero
  stepper.setMaxSpeed(400);       // note 1000 is the fastest speed for this library
  stepper.setAcceleration(500);
  //digitalWrite(EN, HIGH);  // turn off motor drive, write a LOW to turn back on
}


void loop() {
  ReadRawAngle();    //ask the value from the sensor
  correctAngle();    //tare the value
  checkQuadrant();   //check quadrant, check rotations, check absolute angular position
  calculateRPM();    //Calculate RPM
  refreshDisplay();  //Update the OLED if values were changed (TIME CONSUMING!)
  //digitalWrite(EN, LOW);
  Serial.print("rpmValue:");
  Serial.println(rpmValue,2);
  if (rpmValue >= 4.5) {
    while (digitalRead(UPPER_LIMIT) == LOW) {
      rotateClockwise();
    }
  } else if (rpmValue < 4.5) {
    while (digitalRead(LOWER_LIMIT) == LOW) {
      rotateAnti();
    }
  }
}
void rotateClockwise() {
  digitalWrite(DIR, HIGH);
  digitalWrite(STEP, HIGH);
  delayMicroseconds(100);
  digitalWrite(STEP, LOW);
}

void rotateAnti() {
  digitalWrite(DIR, LOW);
  digitalWrite(STEP, LOW);
  delayMicroseconds(100);
  digitalWrite(STEP, HIGH);
}
void ReadRawAngle() {
  //7:0 - bits
  Wire.beginTransmission(0x36);  //connect to the sensor
  Wire.write(0x0D);              //figure 21 - register map: Raw angle (7:0)
  Wire.endTransmission();        //end transmission
  Wire.requestFrom(0x36, 1);     //request from the sensor

  while (Wire.available() == 0)
    ;                     //wait until it becomes available
  lowbyte = Wire.read();  //Reading the data after the request

  //11:8 - 4 bits
  Wire.beginTransmission(0x36);
  Wire.write(0x0C);  //figure 21 - register map: Raw angle (11:8)
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);

  while (Wire.available() == 0)
    ;
  highbyte = Wire.read();

  //4 bits have to be shifted to its proper place as we want to build a 12-bit number
  highbyte = highbyte << 8;  //shifting to left
  //What is happening here is the following: The variable is being shifted by 8 bits to the left:
  //Initial value: 00000000|00001111 (word = 16 bits or 2 bytes)
  //Left shifting by eight bits: 00001111|00000000 so, the high byte is filled in

  //Finally, we combine (bitwise OR) the two numbers:
  //High: 00001111|00000000
  //Low:  00000000|00001111
  //      -----------------
  //H|L:  00001111|00001111
  rawAngle = highbyte | lowbyte;  //int is 16 bits (as well as the word)

  //We need to calculate the angle:
  //12 bit -> 4096 different levels: 360° is divided into 4096 equal parts:
  //360/4096 = 0.087890625
  //Multiply the output of the encoder with 0.087890625
  degAngle = rawAngle * 0.087890625;

  //Serial.print("Deg angle: ");
  //Serial.println(degAngle, 2); //absolute position of the encoder within the 0-360 circle
}

void correctAngle() {
  //recalculate angle
  correctedAngle = degAngle - startAngle;  //this tares the position

  if (correctedAngle < 0)  //if the calculated angle is negative, we need to "normalize" it
  {
    correctedAngle = correctedAngle + 360;  //correction for negative numbers (i.e. -15 becomes +345)
  } else {
    //do nothing
  }
  Serial.print("Corrected angle: ");
  Serial.println(correctedAngle, 2); //print the corrected/tared angle
}

void checkQuadrant() {
  /*
    //Quadrants:
    4  |  1
    ---|---
    3  |  2
  */

  //Quadrant 1
  if (correctedAngle >= 0 && correctedAngle <= 90) {
    quadrantNumber = 1;
  }

  //Quadrant 2
  if (correctedAngle > 90 && correctedAngle <= 180) {
    quadrantNumber = 2;
  }

  //Quadrant 3
  if (correctedAngle > 180 && correctedAngle <= 270) {
    quadrantNumber = 3;
  }

  //Quadrant 4
  if (correctedAngle > 270 && correctedAngle < 360) {
    quadrantNumber = 4;
  }
  Serial.print("Quadrant: ");
  Serial.println(quadrantNumber); //print our position "quadrant-wise"

  if (quadrantNumber != previousquadrantNumber)  //if we changed quadrant
  {
    if (quadrantNumber == 1 && previousquadrantNumber == 4) {
      numberofTurns++;  // 4 --> 1 transition: CW rotation
    }

    if (quadrantNumber == 4 && previousquadrantNumber == 1) {
      numberofTurns--;  // 1 --> 4 transition: CCW rotation
    }
    //this could be done between every quadrants so one can count every 1/4th of transition

    previousquadrantNumber = quadrantNumber;  //update to the current quadrant
  }
  Serial.print("Turns: ");
  Serial.println(numberofTurns,0); //number of turns in absolute terms (can be negative which indicates CCW turns)

  //after we have the corrected angle and the turns, we can calculate the total absolute position
  totalAngle = (numberofTurns * 360) + correctedAngle;  //number of turns (+/-) plus the actual angle within the 0-360 range
  Serial.print("Total angle: ");
  Serial.println(totalAngle, 2); //absolute position of the motor expressed in degree angles, 2 digits
}

void checkMagnetPresence() {
  //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly

  while ((magnetStatus & 32) != 32)  //while the magnet is not adjusted to the proper distance - 32: MD = 1
  {
    magnetStatus = 0;  //reset reading

    Wire.beginTransmission(0x36);  //connect to the sensor
    Wire.write(0x0B);              //figure 21 - register map: Status: MD ML MH
    Wire.endTransmission();        //end transmission
    Wire.requestFrom(0x36, 1);     //request from the sensor

    while (Wire.available() == 0)
      ;                          //wait until it becomes available
    magnetStatus = Wire.read();  //Reading the data after the request

    Serial.print("Magnet status: ");
    Serial.println(magnetStatus, BIN); //print it in binary so you can compare it to the table (fig 21)
  }

  //Status register output: 0 0 MD ML MH 0 0 0
  //MH: Too strong magnet - 100111 - DEC: 39
  //ML: Too weak magnet - 10111 - DEC: 23
  //MD: OK magnet - 110111 - DEC: 55

  Serial.println("Magnet found!");
  delay(1000);
}

void calculateRPM() {
  //This function calculates the RPM based on the elapsed time and the angle of rotation.
  //Positive RPM is CW, negative RPM is CCW. Example: RPM = 300 - CW 300 rpm, RPM = -300 - CCW 300 rpm.
  timerdiff = millis() - rpmTimer;

  if (timerdiff > rpmInterval) {

    //rpmValue = (60000.0/rpmInterval) * (totalAngle - recentTotalAngle)/360.0;
    rpmValue = (60000.0 / timerdiff) * (totalAngle - recentTotalAngle) / 360.0;
    //Formula: (60000/2000) * (3600 - 0) / 360;
    //30 * 10 = 300 RPM. So, in 2 seconds we did 10 turns (3600degrees total angle), then assuming that the speed is constant
    //2 second is 1/30th of a minute, so we multiply the 2 second data with 30 -> 30*10 = 300 rpm.
    //The purpose of the (60000/rpmInterval) is that we always normalize the rounds per rpmInterval to rounds per minute

    //Step rate (steps/s) is assumed with 800 steps/360° microstepping. 1 step = 0.45°
    //1 RPM = 800 steps/minute because of the 800 steps/revolution microstepping
    //We also divide by 60, because we want the units to be in steps/s
    stepRateFromRPM = rpmValue * 800.0 / 60.0;

    //Also, keep in mind that 800 steps/s means that every 1.25 ms (1250 us) we need to do a step.
    //The maximum desired speed has to be kept in mind when you define the interrupt frequency.

    recentTotalAngle = totalAngle;  //Make the totalAngle as the recent total angle.
    rpmTimer = millis();            //Update the timer with the current millis() value

  }
}


void refreshDisplay() {
  //Display layout

  // Accelstepper   AS5600
  //-----------------------------
  // Steps/s        CALCULATED
  // CALCULATED     RPM
  // POSITION       CALCULATED
  // CALCULATED     TOTALANGLE

  if (millis() - OLEDTimer > 10)  //chech if we will update at every 10 ms
  {

    if (totalAngle != previoustotalAngle)  //if there's a change in the position*
    {

      //LINE 1 - STEPS/S
      //Accelstepper speed (steps/s)

      oled.setCursor(70, 0);
      oled.print("        ");
      oled.setCursor(70, 0);
      oled.print(-1.0 * stepRateFromRPM, 1);  //Calculated from AS5600
      //-----------------------------------------------

      oled.setCursor(70, 1);
      oled.print("        ");
      oled.setCursor(70, 1);
      oled.print(-1.0 * rpmValue, 1);  //Measured by AS5600
      //--------------------------------------------------------------------
      oled.setCursor(70, 2);
      oled.print("        ");
      oled.setCursor(70, 2);
      oled.print(-1 * totalAngle / 0.45, 0);  //Calculated position from AS5600 total angle
      //--------------------------------------------------------------------
      //LINE 4 - ANGLE

      oled.setCursor(70, 3);
      oled.print("        ");
      oled.setCursor(70, 3);
      oled.print(-1 * totalAngle, 2);
      //--------------------------------------------------------------------

      OLEDTimer = millis();             //reset timer
      previoustotalAngle = totalAngle;  //update the previous value
    }
    //If the code enters this part, it takes about 40-50 ms to update the display
    //800 steps/s = 60 rpm = 360°/s -> 90° degree is 250 ms. (Why 90°? -> quadrants!)
    //So, this code is reliable up to 240 RPM without any "optimization".
    else {
      //skip
    }
  }
  //*idea: you can define a certain tolerance for the angle so the screen will not flicker
  //when there is a 0.08 change in the angle (sometimes the sensor reads uncertain values)
}
void findEnd(byte limitSwitch, byte moveDirection) {
  if (moveDirection == 0) maxTravel = maxTravel * -1;  // make a negative value
  Serial.print("moving to ");
  Serial.println(maxTravel);  // uncomment for debug
  stepper.moveTo(maxTravel);
  while (digitalRead(limitSwitch) == HIGH) {
    stepper.run();
  }
  stepper.stop();                                                // Stop as fast as possible
  Serial.println("motor at low end with limit switch touched");  // uncomment for debug
}

void backup(byte limitSwitch, byte moveDirection) {    // move off the end spot
  if (moveDirection != 0) maxBackup = maxBackup * -1;  // make a negative value
  Serial.print("Backup moving to ");
  Serial.println(maxBackup);  // uncomment for debug
  stepper.moveTo(maxBackup);
  while (digitalRead(limitSwitch) == LOW) {
    stepper.run();
  }
  stepper.stop();                                                    // Stop as fast as possible
  Serial.println("motor at low end with limit switch not touched");  // uncomment for debug
}

// move to a position with the limit switches monitored
boolean moveSafe(long moveToHere) {  // returns true is limit switch tripped otherwise returns false
  boolean tripped = false;
  stepper.moveTo(moveToHere);
  while (stepper.distanceToGo() != 0 && (digitalRead(LOWER_LIMIT) == HIGH)) {
    stepper.run();
  }
  if ((digitalRead(LOWER_LIMIT) == LOW)) {
    stepper.stop();
    tripped = true;
  }
  return tripped;
}

void recover() {  // recover when a limit switch is triggered
  if (digitalRead(LOWER_LIMIT) == LOW) backup(LOWER_LIMIT, 0);
}

the code output in post #1 is a mess making it difficult to read - try to fix it
put some more print statements in your code displaying intermediate values etc and showing the flow of the program
then upload as text the serial monitor output
what is the host microcontroller?

1 Like

Can you post all your code in the same set of code tags. This might help you.
How to get the best from this from this Forum before you proceed any further.

It tells you about posting code. Go back and edit your first post, using the pencil icon at the bottom of your post, don't post the code again in a new post.

That is not very helpful. Can you describe what you expect from this code and how it differs from what actually happens. The words work and doesn't work do not cut it here.

Have you written this code all at once and then tested it for the first time? For example have you written any code that just tests you can receive data from your AS5600 ?

Also it is called the loop function and NOT the void loop, I corrected the title but it is not a very good title because it is very vague.

It is normal to include a schematic (hand drawn is fine Fritzing is not) so we can see how the code interacts with the hardware.

1 Like

I did try AS5600 step and rpm monitoring and homing command separately and they work just fine.
But when I combine the program and setup an algorithm in loop function, it doesn't work, it just stays at idle position.

So what you do in that case is to add Serial.print statements that print out the various values in the code while it runs. This way you can concentrate on the sections where you see unexpected values.

1 Like

Homing and Sensor data measuring monitoring works after combining but loop function doesn't work.
Here are the hand drawn schematics and real images.

Sorry but forum doesn't allow me to add more than one image.

Serial print command is showing the correct result of Stepper calibration and AS5600 in oled correctly.

Still don't know what that means. The loop function will always "work", it is just not doing what you expect. Put more Serial print statements to find out why.

2 Likes

seem to remember asking for that back in post # 2

2 Likes

Hey Horace, I have added Serial.print statements as you have suggested, and the results are as expected.
Now the only problem is with If and else statement in the loop function which is not behaving as it ought to.
Please have a look and suggest?
(Added an image of serial monitor.)

avoid uploading screen shots
upload serial monitor output text adding comments where it has gone wrong and what you expected it to do
are you sure the hardware works, e.g.

    while (digitalRead(UPPER_LIMIT) == LOW) {
      rotateClockwise();

does the test of UPPER_LIMIT work?
when you call rotateClockwise() does it work?
etc etc

1 Like

I did another iteration where I added a Serial.print statement under while condition in loop function to analyze the state of the limit switches and it turns out the serial monitor result is same as the previous one.

And to answer question
Yes, the hardware works till stepper calibration.
LOWER_LIMIT works as it is used in homing the stepper motor.
But
When I call rotateClockwise and roateAnti, it doesn't work.

I expected that after reading certain rpmValue, the program will go under If and else condition and work as directed but it just stays idle.

I have found the thing I was missing. Thank you, Horace and Grumpy_Mike to question the program which made me reanalyze my program from the problem statement.

So are you going to complete the thread by describing exactly what it was that you found wrong. This will help others, in the future that are having a similar problem.

Then mark your problem as "solved" in the first post and credit a post from Horace as the post that allowed you to solve it.

The code was right from the beginning.
The only mistake I did was the logic of the upper limit switch.

Just a final point on your diagram in post #6.

Your OLED is connected to 3V3 and is on the same I2C bus as the AS5600 and an Arduino. This will cause your OLED to experience a bigger pull up signal than its supply voltage, which in the end will damage it. If your OLED board has a regulator on it, and has a 5V input then use that, otherwise I suggest you use an
I2C bi-directional level shifter.

1 Like

In final prototype, I will remove the Oled screen. I only installed it to check the stepper motor steps and rpm instead of looking at serial monitor. But thanks for the suggestions, I will definitely keep that in my mind.

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.