我利用手上沒在使用的三個滑軌和三個57步進馬達架構一個XYZ三軸方向的移動平台, 並使用ESP32作為3Axis controller控制移動方向. 有單軸指令, 例如 :X10 :Y10 :Z10跟三軸連動指令:P10,10,10
I used three unused slide rails and three 57 stepper motors to construct a mobile platform in the XYZ three-axis direction, and used ESP32 as the 3Axis controller to control the movement direction. There are single-axis instructions, for example: X10:Y10: Z10 and three-axis linkage command: P10,10,10
Architectures:
Circuit:
#define MACHINE_NAME "ESP32_V4"
#define X_STEP_PIN GPIO_NUM_12
#define X_DIRECTION_PIN GPIO_NUM_14
#define Y_STEP_PIN GPIO_NUM_26
#define Y_DIRECTION_PIN GPIO_NUM_15
#define Z_STEP_PIN GPIO_NUM_27
#define Z_DIRECTION_PIN GPIO_NUM_33
#define X_LIMIT_PIN GPIO_NUM_17
#define Y_LIMIT_PIN GPIO_NUM_4
#define Z_LIMIT_PIN GPIO_NUM_16
// OK to comment out to use pin for other features
#define STEPPERS_DISABLE_PIN GPIO_NUM_13
#define SPINDLE_TYPE SpindleType::PWM
#define SPINDLE_OUTPUT_PIN GPIO_NUM_2 // labeled SpinPWM
#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl
#define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist
#define COOLANT_FLOOD_PIN GPIO_NUM_25 // labeled Flood
#define PROBE_PIN GPIO_NUM_32 // labeled Probe
Fundamental:
DM542 microstep driver
Reference the previous article link on blog:
Arduino 控制三顆馬達
YouTube Demo:
ESP32 Code:
#include <AccelStepper.h>
#include <MultiStepper.h>
//--------- Flag structure --------------------------------------
typedef struct _vFlag
{
uint8_t BTFlag = 0;
uint8_t initial_Flag = 0;
} vFlag;
vFlag *flag_Ptr;
vFlag flag;
//------LED------------------
#define LED_BUILTIN 2
char ctemp[20];
//--------- uart structure --------------------------------------
//----------uart--------------
#define LINE_BUFFER_LENGTH 64
typedef struct _vUart
{
char c;
int lineIndex = 0;
int line1Index = 0;
int BTlineIndex = 0;
bool lineIsComment;
bool lineSemiColon;
char line[128];
char BTline[20];
String inputString;
String BTinputString;
int V[16];
char ctemp[30];
char I2C_Data[80];
float Voltage[16];
int Buffer[128];
} vUart;
vUart *Uart_Ptr;
vUart Uart;
//--------- motor structure --------------------------------------
typedef struct _axis_state_t
{
uint8_t pulPin[6]; //--X,Y,Z,A,B,C
uint8_t dirPin[6];
uint8_t enaPin[6];
uint8_t HomePin[6];
long RotatorAngle[6];
boolean AXISFlag[6];
uint8_t HomeFlag[6];
uint8_t RotatorFlag[6];
float FeedRate[6];
float MaxSpeed[6];
float Acceleration[6];
int AXISNumber[2];
int DC_Speed[6];
} axis_state_t;
axis_state_t *gc_Ptr;
axis_state_t gc;
//----------multi motor ---------------------------------------
MultiStepper steppers;
long positions[3];
#define Stepper_ENABLE 13
#define X_LIMIT_PIN 17
#define Y_LIMIT_PIN 4
#define Z_LIMIT_PIN 16
AccelStepper stepperX (1,12,14); // name of stepper motor (1 = driver, pin 2 = pulse, pin 3 = direction)
AccelStepper stepperY (1,26,15); // name of stepper motor (1 = driver, pin 4 = step, pin 5 = direction)
AccelStepper stepperZ (1,27,33); // name of stepper motor (1 = driver, pin 6 = step, pin 7 = direction)
//------------------------------------------------------------------------------
//-------------------------------------
TaskHandle_t huart;
TaskHandle_t hfunction;
TaskHandle_t hmultimotor; //TaskHandler
void MultiMotorTask(void *pvParameters);
void vUARTTask(void *pvParameters);
void vFunctionTask(void *pvParameters);
//------------------------------------------------------------------------------
void initial()
{
Serial.println(F("Create Task"));
//----------------------------------------------------------------------
xTaskCreatePinnedToCore(
vUARTTask, "UARTTask" // A name just for humans
,
1024 // This stack size can be checked & adjusted by reading the Stack Highwater
,
NULL, 3 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
,
&huart //handle
,
0);
//----------------------------------------------------------------------
initMultiMotorTask();
//----------------------------------------------------------------------
}
void initMultiMotorTask(void)
{
// Create the task
xTaskCreatePinnedToCore(MultiMotorTask, "MultiMotorTask", 1024, NULL, 3, &hmultimotor, 1);
// Check the results
if (hmultimotor == NULL)
{
Serial.println("Create MultiMotor task failed");
}
else
{
Serial.println("MultiMotor task up and running");
vTaskSuspend(hmultimotor);
}
}
//------------------------------------------------------------------------------
void MultiMotorTask(void *pvParameters)
{
(void)pvParameters;
while(1)
{
digitalWrite(Stepper_ENABLE, LOW);
if (gc.AXISFlag[0] || gc.AXISFlag[1] || gc.AXISFlag[2])
{
if (gc.RotatorFlag[0] == 1)
{
//Serial.println( "1" );
gc.RotatorAngle[0] = gc.RotatorAngle[0] * 500;
//gc.RotatorAngle[0] = gc.RotatorAngle[0] * 100; //F4設定
positions[0] = gc.RotatorAngle[0];
}
else
{
positions[0] = stepperX.currentPosition();
}
if (gc.RotatorFlag[1] == 1)
{
//Serial.println( "2" );
gc.RotatorAngle[1]=gc.RotatorAngle[1]*500;//F4設定
positions[1] = gc.RotatorAngle[1];
}
else
{
positions[1] = stepperY.currentPosition();
}
if (gc.RotatorFlag[2] == 1)
{
//Serial.println( "3" );
gc.RotatorAngle[2] = gc.RotatorAngle[2] * 100;
//gc.RotatorAngle[2] = gc.RotatorAngle[2] * 250;//F4設定
positions[2] = gc.RotatorAngle[2];
}
else
{
positions[2] = stepperZ.currentPosition();
}
//----multi motor
//positions[0]=gc.RotatorAngle[0];
//positions[1]=gc.RotatorAngle[1];
//vTaskSuspend(hled);
steppers.moveTo(positions);
steppers.runSpeedToPosition();
//steppers.run();
if (abs(stepperX.currentPosition()) == abs(gc.RotatorAngle[0]))
{
gc.AXISFlag[0] = false;
if (gc.RotatorFlag[0] == 1)
{
gc.AXISNumber[0]--;
}
Serial.println( "X_OK" );
//vTaskSuspend( hmultimotor );
}
if (abs(stepperY.currentPosition()) == abs(gc.RotatorAngle[1]))
{
gc.AXISFlag[1] = false;
if (gc.RotatorFlag[1] == 1)
{
gc.AXISNumber[0]--;
}
Serial.println( "Y_OK" );
//vTaskSuspend( hmultimotor );
}
if (abs(stepperZ.currentPosition()) == abs(gc.RotatorAngle[2]))
{
gc.AXISFlag[2] = false;
if (gc.RotatorFlag[2] == 1)
{
gc.AXISNumber[0]--;
}
Serial.println( "Z_OK" );
//vTaskSuspend( hmultimotor );
}
if (gc.AXISNumber[0] == 0)
{
//vTaskResume(hled);
for (int i = 0; i < 6; i++)
{
gc.RotatorFlag[i] = 0;
}
//vTaskSuspend(hmultimotor);
}
}
vTaskDelay(1);
//vTaskDelay(configTICK_RATE_HZ);
}
}
void initMultiMotorSetup(void)
{
gc.pulPin[0] = 26;
gc.dirPin[0] = 25;
//gc.HomePin[0] = 8;
//gc.enaPin[0]=8;
gc.pulPin[1] = 32;
gc.dirPin[1] = 33;
//gc.enaPin[1]=8;
//gc.HomePin[1] = 9;
gc.pulPin[2] = 34;
gc.dirPin[2] = 35;
//gc.enaPin[2]=8;
//gc.HomePin[2] = 10;
for (int i = 0; i < 6; i++)
{
//pinMode(gc.pulPin[i], OUTPUT);
//pinMode(gc.dirPin[i], OUTPUT);
//pinMode(gc.enaPin[i], OUTPUT);
//digitalWrite(gc.enaPin[i], HIGH);
gc.RotatorAngle[i] = 0;
gc.AXISFlag[i] = false;
gc.HomeFlag[i] = 0;
gc.RotatorFlag[i] = 0;
gc.FeedRate[i] = 1000;
gc.MaxSpeed[i] = 4000;
gc.Acceleration[i] = 800;
}
gc.AXISNumber[0] = 0;
gc.AXISNumber[1] = 0;
pinMode(Stepper_ENABLE, OUTPUT);
digitalWrite(Stepper_ENABLE, HIGH);
//pinMode(26, OUTPUT);
stepperX.setMaxSpeed(4000);
stepperX.setAcceleration(1000);
stepperX.setSpeed(4000); //:f4000,4000,4000
stepperY.setMaxSpeed(4000); //:ms8000,8000,8000
stepperY.setAcceleration(1000);
stepperY.setSpeed(1000);
stepperZ.setMaxSpeed(4000);
stepperZ.setAcceleration(800);
stepperZ.setSpeed(2000);
// Then give them to MultiStepper to manage
steppers.addStepper(stepperX);
steppers.addStepper(stepperY);
steppers.addStepper(stepperZ);
}
//-------------------------------------------------
void setup()
{
Serial.begin(9600);
Serial.println(F("init"));
initial();
initMultiMotorSetup();
}
//-----------------------------------------
void loop()
{
Serial.print(F("Main at core:"));
Serial.println(xPortGetCoreID());
while(1)
{
vTaskDelay(1);
}
}
//----------------------------------------
void processCommand(char *data)
{
int len, xlen, ylen, zlen, alen;
int tempDIO;
String stemp;
len = Uart.inputString.length();
//memset(ctemp, 0, sizeof(ctemp));
//---------------------------------------
if (strstr(data, "VER") != NULL)
{
Serial.println(F("ESP32_20231226"));
}
//-----------Motor----------------------
if (strstr(data, ":P") != NULL)
{
//:p200,100,300
for (int i = 2; i < len; i++)
{
ctemp[i - 2] = data[i];
if (data[i] == ',')
{
xlen = i;
ctemp[xlen] = '\0';
gc.RotatorAngle[0] = atoi(ctemp);
//Serial.println(xlen);
//Serial.println(gc.RotatorAngle[0]);
break;
}
}
int j = (xlen + 1);
for (int i = j; i < len; i++)
{
ctemp[i - j] = data[i];
if (data[i] == ',')
{
ylen = i;
ctemp[ylen] = '\0';
gc.RotatorAngle[1] = atoi(ctemp);
//Serial.print("debug- ");
//Serial.println(gc.RotatorAngle[1]);
break;
}
}
int k = (ylen + 1);
for (int i = k; i < len; i++)
{
zlen = i;
ctemp[i - k] = data[i];
}
ctemp[zlen] = '\0';
gc.RotatorAngle[2] = atoi(ctemp);
//Serial.println(zlen);
//Serial.println(gc.RotatorAngle[2]);
gc.AXISFlag[0] = true;
gc.AXISFlag[1] = true;
gc.AXISFlag[2] = true;
gc.RotatorFlag[0] = 1;
gc.RotatorFlag[1] = 1;
gc.RotatorFlag[2] = 1;
gc.AXISNumber[0] = 3;
vTaskResume(hmultimotor);
}
if (strstr(data, ":F"))
{
//:f4000,4000,2000
for (int i = 2; i < len; i++)
{
Uart.ctemp[i - 2] = data[i];
if (data[i] == ',')
{
xlen = i;
Uart.ctemp[xlen] = '\0';
gc.FeedRate[0] = atoi(Uart.ctemp);
//Serial.println(xlen);
//Serial.println(gc.RotatorAngle[0]);
break;
}
}
int j = (xlen + 1);
for (int i = j; i < len; i++)
{
Uart.ctemp[i - j] = data[i];
if (data[i] == ',')
{
ylen = i;
Uart.ctemp[ylen] = '\0';
gc.FeedRate[1] = atoi(Uart.ctemp);
//Serial.println(ylen);
//Serial.println(gc.RotatorAngle[1]);
break;
}
}
int k = (ylen + 1);
for (int i = k; i < len; i++)
{
zlen = i;
Uart.ctemp[i - k] = data[i];
}
Uart.ctemp[zlen] = '\0';
gc.FeedRate[2] = atoi(Uart.ctemp);
stepperX.setSpeed(gc.FeedRate[0]);
stepperY.setSpeed(gc.FeedRate[1]);
stepperZ.setSpeed(gc.FeedRate[2]);
}
if (strstr(data, ":MS"))
{
//float MaxSpeed[6];
//:p200,100,300
for (int i = 2; i < len; i++)
{
Uart.ctemp[i - 2] = data[i];
if (data[i] == ',')
{
xlen = i;
Uart.ctemp[xlen] = '\0';
gc.MaxSpeed[0] = atoi(Uart.ctemp);
//Serial.println(xlen);
//Serial.println(gc.RotatorAngle[0]);
break;
}
}
int j = (xlen + 1);
for (int i = j; i < len; i++)
{
Uart.ctemp[i - j] = data[i];
if (data[i] == ',')
{
ylen = i;
Uart.ctemp[ylen] = '\0';
gc.MaxSpeed[1] = atoi(Uart.ctemp);
//Serial.println(ylen);
//Serial.println(gc.RotatorAngle[1]);
break;
}
}
int k = (ylen + 1);
for (int i = k; i < len; i++)
{
zlen = i;
Uart.ctemp[i - k] = data[i];
}
Uart.ctemp[zlen] = '\0';
gc.MaxSpeed[2] = atoi(Uart.ctemp);
stepperX.setMaxSpeed(gc.MaxSpeed[0]);
stepperY.setMaxSpeed(gc.MaxSpeed[1]);
stepperZ.setMaxSpeed(gc.MaxSpeed[2]);
}
if (strstr(data, ":ACC"))
{
//float Acceleration[6];
//:ACC1000,1000,1000
for (int i = 2; i < len; i++)
{
Uart.ctemp[i - 2] = data[i];
if (data[i] == ',')
{
xlen = i;
Uart.ctemp[xlen] = '\0';
gc.Acceleration[0] = atoi(Uart.ctemp);
//Serial.println(xlen);
//Serial.println(gc.RotatorAngle[0]);
break;
}
}
int j = (xlen + 1);
for (int i = j; i < len; i++)
{
Uart.ctemp[i - j] = data[i];
if (data[i] == ',')
{
ylen = i;
Uart.ctemp[ylen] = '\0';
gc.Acceleration[1] = atoi(Uart.ctemp);
//Serial.println(ylen);
//Serial.println(gc.RotatorAngle[1]);
break;
}
}
int k = (ylen + 1);
for (int i = k; i < len; i++)
{
zlen = i;
Uart.ctemp[i - k] = data[i];
}
Uart.ctemp[zlen] = '\0';
gc.Acceleration[2] = atoi(Uart.ctemp);
stepperX.setAcceleration(gc.Acceleration[0]);
stepperY.setAcceleration(gc.Acceleration[1]);
stepperZ.setAcceleration(gc.Acceleration[2]);
}
if (strstr(data, ":X") != NULL)
{
gc.HomeFlag[0] = 3;
gc.AXISFlag[0] = true;
for (int i = 2; i < len; i++)
{
ctemp[i - 2] = data[i];
}
ctemp[len - 2] = '\0';
gc.RotatorAngle[0] = atoi(ctemp);
gc.RotatorFlag[0] = 1;
//Serial.println(gc.RotatorAngle[0]);
gc.AXISNumber[0] = 1;
vTaskResume(hmultimotor);
}
if (strstr(data, ":Y") != NULL)
{
gc.AXISFlag[1] = true;
for (int i = 2; i < len; i++)
{
ctemp[i - 2] = data[i];
}
ctemp[len - 2] = '\0';
gc.RotatorAngle[1] = atoi(ctemp);
//Serial.println(gc.RotatorAngle[1]);
gc.RotatorFlag[1] = 1;
gc.AXISNumber[0] = 1;
vTaskResume(hmultimotor);
}
if (strstr(data, ":Z") != NULL)
{
gc.AXISFlag[2] = true;
for (int i = 2; i < len; i++)
{
ctemp[i - 2] = data[i];
}
ctemp[len - 2] = '\0';
gc.RotatorAngle[2] = atoi(ctemp);
//Serial.println(gc.RotatorAngle[2]);
gc.RotatorFlag[2] = 1;
gc.AXISNumber[0] = 1;
vTaskResume(hmultimotor);
}
}
//-----------------------------------------
//-------------------------------------------
void vUARTTask(void *pvParameters)
{
(void)pvParameters;
Serial.print(F("UARTTask at core:"));
Serial.println(xPortGetCoreID());
for (;;)
{
while (Serial.available() > 0)
{
Uart.c = Serial.read();
if ((Uart.c == '\n') || (Uart.c == '\r'))
{ // End of line reached
if (Uart.lineIndex > 0)
{ // Line is complete. Then execute!
Uart.line[Uart.lineIndex] = '\0'; // Terminate string
//Serial.println( F("Debug") );
//Serial.println( Uart.inputString );
processCommand(Uart.line);
Uart.lineIndex = 0;
Uart.inputString = "";
}
else
{
// Empty or comment line. Skip block.
}
Uart.lineIsComment = false;
Uart.lineSemiColon = false;
Serial.println(F("ok>"));
}
else
{
//Serial.println( c );
if ((Uart.lineIsComment) || (Uart.lineSemiColon))
{
if (Uart.c == ')')
Uart.lineIsComment = false; // End of comment. Resume line.
}
else
{
if (Uart.c == '/')
{ // Block delete not supported. Ignore character.
}
else if (Uart.c == '~')
{ // Enable comments flag and ignore all characters until ')' or EOL.
Uart.lineIsComment = true;
}
else if (Uart.c == ';')
{
Uart.lineSemiColon = true;
}
else if (Uart.lineIndex >= LINE_BUFFER_LENGTH - 1)
{
Serial.println("ERROR - lineBuffer overflow");
Uart.lineIsComment = false;
Uart.lineSemiColon = false;
}
else if (Uart.c >= 'a' && Uart.c <= 'z')
{ // Upcase lowercase
Uart.line[Uart.lineIndex] = Uart.c - 'a' + 'A';
Uart.lineIndex = Uart.lineIndex + 1;
Uart.inputString += (char)(Uart.c - 'a' + 'A');
}
else
{
Uart.line[Uart.lineIndex] = Uart.c;
Uart.lineIndex = Uart.lineIndex + 1;
Uart.inputString += Uart.c;
}
}
}
} //while (Serial.available() > 0)
vTaskDelay(1);
}
}
沒有留言:
張貼留言