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);
}