Прошивка Ардуино для станка с ЧПУ, драйверы DM420A, двигатели 17HS3404N
Краткое описание
- Контроллер: Ардуино
- Драйверы ШД: DM420A, TB6560A, драйверы с управлением DIR/PUL (CW/CLK)
- Шаговые двигатели: 17HS3404N, 17HS2408, 42HS40, двигатели в корпусе Nema
- Назначение: пример интерполяции дуги
Подробное описание
Прошивка для платы Ардуино написана на языке Processing в среде разработки Arduino 1.0.5 r2 в операционной системе Windows.
Эта прошивка предназначена для Arduino (Mega, UNO, Nano) и работает с 2-хосевым станком с ЧПУ. Можно использовать любые драйверы для управления шаговыми двигателями с двухконтактным контролем DIR/PUL (иногда CW/CLK соответственно), например драйверы шаговых двигателей DM420A или драйверы на микросхеме TB6560A.
Используются любые подходящие шаговые двигатели, например 17HS3404N, 17HS2408, 42HS40, двигатели в корпусе Nema.
Прошивка может использоваться для как пример интерполяции дуги окружности.
Внимание! Прошивка не совместима с программой ECNC для управления станком с ЧПУ. Команды вводятся только из монитора.
Пример команды: 1000,0,1000,1;
Описание команды: первые два числа - координаты X и Y в шагах ШД от текущего положения. Третье число - радиус окружности, также в шагах шагового двигателя. Четвёртое число (1 или -1) - положение выпуклости дуги.
Если диаметр окружности меньше расстояния между текущей точкой и конечным положением или четвёртый параметр не равен 1 или -1, то инструмент пойдёт по прямой.
Исходный код прошивки Ардуино
int motorPins[3][2] = {{8,9},{10,11},{12, 13}};
int count;
int count2[3] = {0,0,0};
int val = 0;
int rot=0;
int incomingByte = 0;
int sign=1;
long delayTime;
double stp; //дельта между шагами по x
//Процедура настройки прошивки
void setup() {
int i;
Serial.begin(9600);
for (i=0; i<3; i++) {
for (count = 0; count < 4; count++) {
pinMode(motorPins[i][count], OUTPUT); //установка режима работы цифровых pin'ов Ардуино
}
}
delayTime=2000;
stp=10; //количество шагов дискретизации перемещения инструмента
}
//Поворот двигателя с номерм sm на один шаг вперёд
void moveForward(int sm) {
digitalWrite(motorPins[sm][1], HIGH); //Задаём направление
digitalWrite(motorPins[sm][0], HIGH);
digitalWrite(motorPins[sm][0], LOW);
}
//Поворот двигателя с номерм sm на один шаг назад
void moveBackward(int sm) {
digitalWrite(motorPins[sm][1], LOW);
digitalWrite(motorPins[sm][0], HIGH);
digitalWrite(motorPins[sm][0], LOW);
}
//Задержка в микосекундах
void delayMicros(long wt){
unsigned long mls;
unsigned int mks;
mls=(unsigned long)(wt / 1000);
mks=(unsigned int)(wt % 1000);
if (mls>0) delay(mls);
if (mks>0) delayMicroseconds(mks);
}
//Одновременный поворот двигателей 0, 1, 2 на x, y, z шагов соответственно
void MoveSM(long x, long y, long z) {
long c[3], c2[3];
double c1[3], d[3];
long m, i;
boolean flg;
c[0] = x;
c[1] = y;
c[2] = z;
m = 1;
for (i=0; i<3; i++) {
if (m < abs(c[i])) m = abs(c[i]);
}
for (i=0; i<3; i++) {
c1[i] = 0;
d[i] = 1.0 * c[i] / m;
c2[i] = 0;
}
flg = false;
for (i=0; i<3; i++) {
if (abs(c1[i]) < abs(c[i])) flg=true;
}
while (flg) {
flg=false;
for (i=0; i<3; i++) {
if (abs(c1[i]) < abs(c[i]))
c1[i] += d[i];
if (abs(c1[i]) - abs(c2[i]) >= 0.5) {
if (c[i]>0) {
c2[i]++;
moveForward(i);
} else {
c2[i]--;
moveBackward(i);
}
}
if (abs(c1[i]) < abs(c[i])) flg=true;
}
delayMicros(delayTime);
}
}
void MoveArc(long x, long y, double r, long v) {
double sinA, cosA, d;
double x0, y0, x2, y2;
long x1, y1, x3, y3;
d=sqrt(x*x + y*y);
sinA=y/d; cosA=x/d;
x0=d/2; y0=-sqrt(r*r - d*d/4);
x1=0; y1=0;
for (x2=stp; x2<d; x2+=stp) {
y2=y0+sqrt(r*r-(x2-x0)*(x2-x0));
x3=round(x2*cosA-v*y2*sinA);
y3=round(x2*sinA+v*y2*cosA);
if (x1!=x3 || y1!=y3) MoveSM(x3-x1,y3-y1,0);
x1=x3; y1=y3;
}
if (x1!=x || y1!=y) MoveSM(x-x1,y-y1,0);
}
//Основной цикл
void loop() {
if (Serial.available() > 0) { //Пришла команда
long c[4]={0,0,0,0};
int i;
sign=1;
i=0;
incomingByte = Serial.read();
while (incomingByte!=';') { //Читаем входящую строку, признак конца строки знак "точка с запятой"
if (c[i]==0) {
if (incomingByte=='-')
sign=-1;
}
if (incomingByte==',') {
c[i]*=sign;
sign=1;
i++;
} else if (incomingByte>='0' && incomingByte<='9') {
c[i]=c[i]*10+incomingByte-'0';
}
while (Serial.available() == 0) {
delayMicroseconds(1); //Ждём очередной символ, если не пришел
}
incomingByte = Serial.read();
}
c[i]*=sign;
//if (c[3]>0) delayTime=c[3];
if (4*c[2]*c[2]>c[0]*c[0]+c[1]*c[1] && abs(c[3])==1)
MoveArc(c[0],c[1],c[2],c[3]);
else
MoveSM(c[0],c[1],0); //Вращаем двигатели на заданное число шагов
Serial.println("OK"); //Отправляем компьютеру сообщение "OK", значит можно высылать новую команду
}
else
delayMicroseconds(1); //Если ничего не пришло, ждём 1 миллисекуду.
}