MINIQ BG6300 milling table with stepper motors and AS5600 encoder
In this video I show you the final iteration of my stepper motor driven milling table project. I added the AS5600 magnetic position encoders to each stepper motors so I know the exact position of the shaft which I can use to calculate the displacement of the table. Of course I could do the same using the number of steps, but it is not reliable and I also wanted to do this as an exercise and proof of concept for future projects. Unfortunately, I mixed up the wiring for the joystick, so that part of the demo is not as good as I wanted to, but I explain everything about the mistake I did.
Relevant topics:
MINIQ BG6300 milling table with stepper motors
Schematics
There are several things to keep in mind regarding this drawing. Usually these Nokia 5110 displays do not tolerate +5 V! I used +3.3 V throughout the whole system, but if you are planning to use it with +5 V, make sure that you have the proper display, or use a level shifter (the +5 V tolerance applies on every pins!). In this drawing I replaced the pins of the joystick as compared to the pins in the source code in order to decrease the mess in the wiring! Keep in mind that the source code has different pins for the X and Y-axis and for the button pins on the joystick. Regarding the stepper motor part, make sure that the ground coming from the power supply is connected to the ground of the microcontroller-circuit! The power supply can be between 9-45 V and it is suggested to use a 100 uF capacitor between the VMOT and the GND pins. Also, make sure that the sleep (SLP) and reset (RST) pins are pulled up to the +3.3 V (or +5 V). The wiring demonstrated here sets the microstepping to 400 steps/turn (MS0 = high, MS1 = MS2 = low). If the DIR pin of the AS5600 boards are not used, they have to be connected to the ground (GND).
Source code
#include <Wire.h> //i2C #include <SPI.h> #include <AccelStepper.h> //accelstepper library AccelStepper stepper(1, PB11, PB10); // direction PB10, step PB11 AccelStepper stepper2(1, PB1, PB0); // direction PB0, step PB1 //Pins const byte analog_X_pin = PA6; //x-axis joystick readings const byte analog_Y_pin = PA3; //y-axis joystick readings const byte joystickSW_pin = PB12; //Joystick button //Variables int analog_X = 0; //x-axis value int analog_Y = 0; //y-axis value //int xvalue = 0; //just for testing purposes //int yvalue = 0; float xDistanceMM = 0; //distance travelled by the x-axis float yDistanceMM = 0; //distance travelled by the y-axis int buttonTimer = 0; //stores a reading from millis() - used for "debouncing" the joystick's button int updateTimer = 0; //stores a reading from millis() - used as a non-blocking update interval volatile int buttonMenuCounter = 0; //Volatile -> It will be changed in the ISR() int prevButton = 1; //It stores the previous state of the menu position //---------------------------------------------------------------------- //The LCD-related code was based on the following library: //https://github.com/cbm80amiga/N5110_GUI_encoder_demo_STM32 //Defining the pins for the Nokia 5110 LCD #define N5110_RST PA0 #define N5110_CS PA4 #define N5110_DC PA1 #define N5110_BACKLIGHT PA2 //Nokia pins: //RST: PA0 //CS: PA4 //DC: PA1 //BL: PA2 (optional) //DIN: PA7 //CLK: PA5 #include "math.h" #include "N5110_SPI.h" #if USESPI==1 #endif N5110_SPI lcd(N5110_RST, N5110_CS, N5110_DC); // RST,CS,DC #include "c64enh_font.h" //smaller font #include "term9x14_font.h" //larger font char buf[25]; //buffer array for printing on the LCD //--------------------------------------------------------------------------- //Magnetic sensor things - ENC 1 int magnetStatus_1 = 0; //value of the status register (MD, ML, MH) int lowbyte_1; //raw angle 7:0 word highbyte_1; //raw angle 7:0 and 11:8 int rawAngle_1; //final raw angle float degAngle_1; //raw angle in degrees (360/4096 * [value between 0-4095]) int quadrantNumber_1, previousquadrantNumber_1; //quadrant IDs float numberofTurns_1 = 0; //number of turns float correctedAngle_1 = 0; //tared angle - based on the startup value float startAngle_1 = 0; //starting angle float totalAngle_1 = 0; //total absolute angular displacement float previoustotalAngle_1 = 0; //for the display printing //--------------------------------------------------------------------------- //Magnetic sensor things - ENC 2 int magnetStatus_2 = 0; //value of the status register (MD, ML, MH) int lowbyte_2; //raw angle 7:0 word highbyte_2; //raw angle 7:0 and 11:8 int rawAngle_2; //final raw angle float degAngle_2; //raw angle in degrees (360/4096 * [value between 0-4095]) int quadrantNumber_2, previousquadrantNumber_2; //quadrant IDs float numberofTurns_2 = 0; //number of turns float correctedAngle_2 = 0; //tared angle - based on the startup value float startAngle_2 = 0; //starting angle float totalAngle_2 = 0; //total absolute angular displacement float previoustotalAngle_2 = 0; //for the display printing float encoderTimer = 0; //used to keep track of the elapsed time for the encoder reading interval int encoderCounter = 0; //keeps track of the active encoder on the i2c MUX void setup() { //SERIAL & WIRE Wire.begin(); //start i2C Wire.setClock(800000L); //fast clock Serial.begin(9600); Serial.println("Test"); //------------------------------------------------------------------------------ //NOKIA 5110 LCD part lcd.init(); //initialize LCD lcd.clrScr(); //clear the whole display lcd.setFont(Term9x14); //Larger font printLCD(); //print some welcome message //---------------------------------------------------------------------------- //PINS pinMode(joystickSW_pin, INPUT_PULLUP); //the joystick pin is an input using the internal pullup resistor attachInterrupt(digitalPinToInterrupt(joystickSW_pin), buttonPressed, CHANGE); //ISR() for the button //---------------------------------------------------------------------------- //Stepper parameters //setting up some default values for maximum speed and maximum acceleration stepper.setMaxSpeed(2000); //SPEED = Steps / second stepper.setAcceleration(100); //ACCELERATION = Steps /(second)^2 stepper.setSpeed(0); //this must be zero to avoid any movements delay(500); //---------------------------------------------------------------------------- stepper2.setMaxSpeed(2000); //SPEED = Steps / second stepper2.setAcceleration(100); //ACCELERATION = Steps /(second)^2 stepper2.setSpeed(0); //this must be zero to avoid any movements delay(500); //---------------------------------------------------------------------------- //Checking the starting angles switchEncoder(); //select the 1st encoder (encoderCounter = 0) checkMagnetPresence_1(); //check the magnet (blocks until magnet is found) ReadRawAngle_1(); //make a reading so the degAngle gets updated startAngle_1 = degAngle_1; //update startAngle with degAngle - for taring switchEncoder(); //select the 2nd encoder (encoderCounter = 1) checkMagnetPresence_2(); //check the magnet (blocks until magnet is found) ReadRawAngle_2(); //make a reading so the degAngle gets updated startAngle_2 = degAngle_2; //update startAngle with degAngle - for taring lcd.clrScr(); //clear the whole display again } void loop() { checkJoyButton(); //check if the button was changed ReadAnalog(); //Read the ADC channels (x, y-axis) stepper.runSpeed(); //step the motors (this will step the motor by 1 step at each loop iteration indefinitely as long as setSpeed is not zero) stepper2.runSpeed(); if (millis() - encoderTimer > 125) //125 ms will be able to make 8 readings in a sec which is enough for 60 RPM { switchEncoder(); //switch to the other encoder ReadRawAngle_1(); //ask the value from the sensor correctAngle_1(); //tare the value checkQuadrant_1(); //check quadrant, check rotations, check absolute angular position // switchEncoder(); //switch to the other encoder ReadRawAngle_2(); //ask the value from the sensor correctAngle_2(); //tare the value checkQuadrant_2(); //check quadrant, check rotations, check absolute angular position encoderTimer = millis(); //save the current millis() value in the variable which "resets" the timer } updateLCD(); //Print the new information } void ReadAnalog() { if (buttonMenuCounter != 3) { //Reading the potentiometers in the joystick: x, y analog_X = analogRead(analog_X_pin); analog_Y = analogRead(analog_Y_pin); //Note: STM32 blue pill has a 12-bit ADC which means that the max value is 4095 (range: 0-4095) //The middle of the potmeter is about 2048 //Behavior of the joystick I use: //Joystick right: analog_x = 0 //Joystick left: analog_x = 4096 //Joystick up: analog_y = 0 //Joystick down: analog_x = 4096 //-------------------------------------------------------------------------------------------------------- if (analog_X > 3000) { stepper.setSpeed(-200); //negative speed - counter-clockwise rotation //xvalue--; //Only for test purposes } else if (analog_X < 1000) { stepper.setSpeed(200); //xvalue++; //Only for test purposes } else { stepper.setSpeed(0); //if the analog value is not larger than 3000 or not smaller than 1000, then we don't move } //---------------------------------------------------------------------------- if (analog_Y > 3000) { stepper2.setSpeed(-200); //yvalue--; //Only for test purposes } else if (analog_Y < 1000) { stepper2.setSpeed(200); //yvalue++; //Only for test purposes } else { stepper2.setSpeed(0); } } else { //buttonMenuCounter == 3 analog_Y = analogRead(analog_Y_pin); if (analog_Y < 1000) //In reality the joystick is pushed UP, but due to the wiring, the value drops when Y goes up { //We reset the corresponding values to zero //Stepper motor stepper.setCurrentPosition(0); //reset the current position to 0 stepper2.setCurrentPosition(0); //also on the other axis //Encoder startAngle_1 = degAngle_1; //New starting angles (new reference points) startAngle_2 = degAngle_2; correctedAngle_1 = 0; //Reset the corrected angle to zero correctedAngle_2 = 0; totalAngle_1 = 0; totalAngle_2 = 0; xDistanceMM = 0; yDistanceMM = 0; //Print some confirmation message on the display lcd.printStr(ALIGN_CENTER, 2, " "); lcd.printStr(ALIGN_CENTER, 2, "RESET"); lcd.printStr(ALIGN_CENTER, 3, " "); lcd.printStr(ALIGN_CENTER, 3, "IS"); lcd.printStr(ALIGN_CENTER, 4, " "); lcd.printStr(ALIGN_CENTER, 4, "DONE!"); } } } void switchEncoder() { if (encoderCounter == 0) //the code enters this part after powering the arduino { Wire.beginTransmission(0x70); //connect to the MUX Wire.write(1 << 0); //write the pins [SD0,SC0] Wire.endTransmission(); // encoderCounter = 1; //update the value, so the code enters the below part at the next iteration } else { Wire.beginTransmission(0x70); Wire.write(1 << 1);//[SD1,SC1] Wire.endTransmission(); // encoderCounter = 0; //update the value, so the code enters the first part at the next iteration } } void ReadRawAngle_1() { //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_1 = 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_1 = Wire.read(); //4 bits have to be shifted to its proper place as we want to build a 12-bit number highbyte_1 = highbyte_1 << 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_1 = highbyte_1 | lowbyte_1; //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_1 = rawAngle_1 * 0.087890625; //Serial.print("Deg angle - ENC 1: "); //Serial.println(degAngle_1, 2); //absolute position of the encoder within the 0-360 circle } void correctAngle_1() { //recalculate angle correctedAngle_1 = degAngle_1 - startAngle_1; //this tares the position if (correctedAngle_1 < 0) //if the calculated angle is negative, we need to "normalize" it { correctedAngle_1 = correctedAngle_1 + 360; //correction for negative numbers (i.e. -15 becomes +345) } else { //do nothing } //Serial.print("Corrected angle - ENC 1: "); //Serial.println(correctedAngle_1, 2); //print the corrected/tared angle } void checkQuadrant_1() { /* //Quadrants: 4 | 1 ---|--- 3 | 2 */ //Quadrant 1 if (correctedAngle_1 >= 0 && correctedAngle_1 <= 90) { quadrantNumber_1 = 1; } //Quadrant 2 if (correctedAngle_1 > 90 && correctedAngle_1 <= 180) { quadrantNumber_1 = 2; } //Quadrant 3 if (correctedAngle_1 > 180 && correctedAngle_1 <= 270) { quadrantNumber_1 = 3; } //Quadrant 4 if (correctedAngle_1 > 270 && correctedAngle_1 < 360) { quadrantNumber_1 = 4; } //Serial.print("Quadrant - ENC 1: "); //Serial.println(quadrantNumber_1); //print our position "quadrant-wise" if (quadrantNumber_1 != previousquadrantNumber_1) //if we changed quadrant { if (quadrantNumber_1 == 1 && previousquadrantNumber_1 == 4) { numberofTurns_1++; // 4 --> 1 transition: CW rotation } if (quadrantNumber_1 == 4 && previousquadrantNumber_1 == 1) { numberofTurns_1--; // 1 --> 4 transition: CCW rotation } //this could be done between every quadrants so one can count every 1/4th of transition previousquadrantNumber_1 = quadrantNumber_1; //update to the current quadrant } //Serial.print("Turns: "); //Serial.println(numberofTurns_1,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_1 = (numberofTurns_1 * 360) + correctedAngle_1; //number of turns (+/-) plus the actual angle within the 0-360 range //Serial.print("Total angle - ENC 1: "); //Serial.println(totalAngle_1, 2); //absolute position of the motor expressed in degree angles, 2 digits } void checkMagnetPresence_1() { //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly while ((magnetStatus_1 & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1 { magnetStatus_1 = 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_1 = Wire.read(); //Reading the data after the request //Serial.print("Magnet status - ENC 1: "); //Serial.println(magnetStatus_1, 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 ReadRawAngle_2() { //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_2 = 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_2 = Wire.read(); //4 bits have to be shifted to its proper place as we want to build a 12-bit number highbyte_2 = highbyte_2 << 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_2 = highbyte_2 | lowbyte_2; //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_2 = rawAngle_2 * 0.087890625; //Serial.print("Deg angle - ENC 2: "); //Serial.println(degAngle_2, 2); //absolute position of the encoder within the 0-360 circle } void correctAngle_2() { //recalculate angle correctedAngle_2 = degAngle_2 - startAngle_2; //this tares the position if (correctedAngle_2 < 0) //if the calculated angle is negative, we need to "normalize" it { correctedAngle_2 = correctedAngle_2 + 360; //correction for negative numbers (i.e. -15 becomes +345) } else { //do nothing } //Serial.print("Corrected angle - ENC 2: "); //Serial.println(correctedAngle_2, 2); //print the corrected/tared angle } void checkQuadrant_2() { /* //Quadrants: 4 | 1 ---|--- 3 | 2 */ //Quadrant 1 if (correctedAngle_2 >= 0 && correctedAngle_2 <= 90) { quadrantNumber_2 = 1; } //Quadrant 2 if (correctedAngle_2 > 90 && correctedAngle_2 <= 180) { quadrantNumber_2 = 2; } //Quadrant 3 if (correctedAngle_2 > 180 && correctedAngle_2 <= 270) { quadrantNumber_2 = 3; } //Quadrant 4 if (correctedAngle_2 > 270 && correctedAngle_2 < 360) { quadrantNumber_2 = 4; } //Serial.print("Quadrant - ENC 2: "); //Serial.println(quadrantNumber_2); //print our position "quadrant-wise" if (quadrantNumber_2 != previousquadrantNumber_2) //if we changed quadrant { if (quadrantNumber_2 == 1 && previousquadrantNumber_2 == 4) { numberofTurns_2++; // 4 --> 1 transition: CW rotation } if (quadrantNumber_2 == 4 && previousquadrantNumber_2 == 1) { numberofTurns_2--; // 1 --> 4 transition: CCW rotation } //this could be done between every quadrants so one can count every 1/4th of transition previousquadrantNumber_2 = quadrantNumber_2; //update to the current quadrant } //Serial.print("Turns - ENC 2: "); //Serial.println(numberofTurns_2,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_2 = (numberofTurns_2 * 360) + correctedAngle_2; //number of turns (+/-) plus the actual angle within the 0-360 range //Serial.print("Total angle - ENC 2: "); //Serial.println(totalAngle_2, 2); //absolute position of the motor expressed in degree angles, 2 digits } void checkMagnetPresence_2() { //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly while ((magnetStatus_2 & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1 { magnetStatus_2 = 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_2 = 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 updateLCD() { if (millis() - updateTimer > 250) //The display is updated every 250 ms { lcd.setFont(c64enh); //set the font (small one) switch (buttonMenuCounter) { case 0: //Steps lcd.printStr(ALIGN_CENTER, 0, "Analog value"); //print new text // lcd.printStr(20, 2, " "); //20th pixel in the 2nd line snprintf(buf, 8, "%d", analog_X); //potmeter value for X lcd.printStr(20, 2, buf); lcd.printStr(20, 4, " "); snprintf(buf, 8, "%d", analog_Y); //potmeter value for Y lcd.printStr(20, 4, buf); break; case 1: //millimeters //Calculating the distance traveled based on the total angular displacement xDistanceMM = -1 * totalAngle_1 / 360.0; //-1 because of the direction yDistanceMM = -1 * totalAngle_2 / 360.0; //Explanation: 1 turn = 360 deg, 1 turn = 1 mm (threaded rod has 1 mm pitch) //Therefore 360 deg = 1 mm or 1 deg = 2.78 um (theoretically!) lcd.printStr(ALIGN_CENTER, 0, "Millimeters"); // lcd.printStr(12, 2, " "); dtostrf(xDistanceMM, 8, 3, buf); //(variable to convert, string length, # of decimals, output (buffer)) lcd.printStr(12, 2, buf); lcd.printStr(12, 4, " "); dtostrf(yDistanceMM, 8, 3, buf); lcd.printStr(12, 4, buf); break; case 2: //degrees lcd.printStr(ALIGN_CENTER, 0, "Degrees"); // lcd.printStr(12, 2, " "); dtostrf(totalAngle_1, 8, 2, buf); lcd.printStr(12, 2, buf); lcd.printStr(12, 4, " "); dtostrf(totalAngle_2, 8, 2, buf); lcd.printStr(12, 4, buf); break; case 3: //Reset home position //Nothing to show here break; } updateTimer = millis(); } } void printLCD() { //welcome message lcd.setFont(c64enh); lcd.printStr(ALIGN_CENTER, 0, "XY-table"); lcd.printStr(ALIGN_CENTER, 1, "Controller"); lcd.printStr(ALIGN_CENTER, 3, "Click the"); lcd.printStr(ALIGN_CENTER, 4, "joystick"); lcd.printStr(ALIGN_CENTER, 5, "to continue..."); } void buttonPressed() { if (millis() - buttonTimer > 1000) //"timer-based" debouncing, you cannot click the button twice within 1 second { buttonMenuCounter++; if (buttonMenuCounter == 4) { buttonMenuCounter = 0; //circular: ...0-1-2-3-0-1-2-3-0... } buttonTimer = millis(); } } void checkJoyButton() { if (buttonMenuCounter != prevButton) //if the button was pressed { lcd.clrScr(); //clear the screen lcd.setFont(c64enh); //set the font (small one) switch (buttonMenuCounter) { case 0: //Analog value lcd.printStr(ALIGN_CENTER, 0, "Analog value"); //print new text // snprintf(buf, 8, "%s", "X: "); lcd.printStr(0, 2, buf); //0th pixel, 2nd line snprintf(buf, 8, "%s", "Y: "); lcd.printStr(0, 4, buf); break; case 1: //millimeters lcd.printStr(ALIGN_CENTER, 0, "Millimeters"); //print new text // snprintf(buf, 8, "%s", "X: "); lcd.printStr(0, 2, buf); snprintf(buf, 8, "%s", "Y: "); lcd.printStr(0, 4, buf); break; case 2: //Degrees lcd.printStr(ALIGN_CENTER, 0, "Degrees"); //print new text // snprintf(buf, 8, "%s", "X: "); lcd.printStr(0, 2, buf); snprintf(buf, 8, "%s", "Y: "); lcd.printStr(0, 4, buf); break; case 3: //Reset home position lcd.printStr(ALIGN_CENTER, 0, "Reset to 0"); //print new text lcd.printStr(ALIGN_CENTER, 2, "Push the"); lcd.printStr(ALIGN_CENTER, 3, "joystick"); lcd.printStr(ALIGN_CENTER, 4, "UP to reset."); break; } prevButton = buttonMenuCounter; } }
