Сервопривод вместо ШД.
- Войдите на сайт для отправки комментариев
Ср, 01/06/2016 - 17:36
Здравствуйте. Помогите пожалуйста. Сам совсем ни-ни. Есть скетч. Управлениие ЧПУ станком. Помогите пожалуйста воткнуть управление сервоприводом вместо оси Z. Заранее спасибо
int motorPins[3][4] = {{2, 3, 4, 5},{6, 7, 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;
int s_p=40;
//Процедура настройки прошивки
void setup() {
int i;
Serial.begin(9600);
pinMode(A2, OUTPUT);
for (i=0; i<3; i++) {
for (count = 0; count < 4; count++) {
pinMode(motorPins[i][count], OUTPUT); //установка режима работы цифровых pin'ов Ардуино
}
}
delayTime=2000; //задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения)
}
//Поворот двигателя с номерм sm на один шаг вперёд в направлении st (-1 или 1)
void moveDir(int sm, int st) {
if ((st==1 ) && (count2[sm]) == 4) count2[sm] = 0;
if ((st==-1) && (count2[sm]) == 1) count2[sm] = 5;
count2[sm]+=st;
switch (count2[sm]) {
case 1:
digitalWrite(motorPins[sm][0],HIGH);
digitalWrite(motorPins[sm][1],LOW);
digitalWrite(motorPins[sm][2],LOW);
digitalWrite(motorPins[sm][3],HIGH);
break;
case 2:
digitalWrite(motorPins[sm][0],LOW);
digitalWrite(motorPins[sm][1],HIGH);
digitalWrite(motorPins[sm][2],LOW);
digitalWrite(motorPins[sm][3],HIGH);
break;
case 3:
digitalWrite(motorPins[sm][0],LOW);
digitalWrite(motorPins[sm][1],HIGH);
digitalWrite(motorPins[sm][2],HIGH);
digitalWrite(motorPins[sm][3],LOW);
break;
case 4:
digitalWrite(motorPins[sm][0],HIGH);
digitalWrite(motorPins[sm][1],LOW);
digitalWrite(motorPins[sm][2],HIGH);
digitalWrite(motorPins[sm][3],LOW);
break;
}
}
//Поворот двигателя с номерм sm на один шаг вперёд
void moveForward(int sm) {
moveDir(sm,1);
}
//Поворот двигателя с номерм sm на один шаг назад
void moveBackward(int sm) {
moveDir(sm,-1);
}
//Задержка в микосекундах
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;
long ms;
long d1, d2, d3, d4, t, dt;
c[0] = x;
c[1] = y;
c[2] = z;
m = 1;
for (i=0; i<2; i++) {
if (m < abs(c[i])) m = abs(c[i]);
}
for (i=0; i<2; i++) {
c1[i] = 0;
d[i] = 1.0 * c[i] / m;
c2[i] = 0;
}
flg = false;
for (i=0; i<2; i++) {
if (abs(c1[i]) < abs(c[i])) flg=true;
}
t=m;
if (z>0 && t>0) {
if (z>=delayTime) {
dt=z; d1=s_p; d2=1;
d3=0;
if (d1==d2) d4=z/2; else d4=z;
} else {
dt=delayTime;
d3=dt*s_p*(dt-z)/dt/2;
d2=d3/dt;
d1=s_p-d2;
d3=d3-d2*dt;
d2=d2+1;
if (d1==d2) d4=dt/2-d3; else d4=dt-d3;
}
} else {
dt=delayTime;
d1=-1; d2=-1; d3=0; d4=0;
}
while (flg) {
flg=false;
for (i=0; i<2; 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;
}
if (z>0) {
if (t==d1) {
delayMicros(d3);
digitalWrite(A2, HIGH);
delayMicros(d4);
}
if (t==d2) {
delayMicros(d4);
digitalWrite(A2, LOW);
delayMicros(d3);
}
if (t!=d1 && t!=d2) {
delayMicros(dt);
}
} else {
delayMicros(dt);
}
t--;
}
}
//Основной цикл
void loop() {
if (Serial.available() > 0) { //Пришла команда
long c[5]={0,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) s_p=c[3]; //количество шагов на 1 пиксель
if (c[4]>0) delayTime=c[4]; //количество шагов на 1 пиксель
MoveSM(c[0],c[1],c[2]); //Вращаем двигатели на заданное число шагов
Serial.println("OK"); //Отправляем компьютеру сообщение "OK", значит можно высылать новую команду
}
else
delayMicroseconds(1); //Если ничего не пришло, ждём 1 микросекунду
}
#include <Stepper.h> // Подключаем библиотеку для управления ШД Stepper Xmotor (250,2,3,4,5); // Инициализируем ШД оси Х, подключенный к pin'ам Arduino 2,3,4,5 Stepper Ymotor (250,6,7,8,9); // Инициализируем ШД оси Y, подключенный к pin'ам Arduino 6,7,8,9 #include <Servo.h> // Подключаем библиотеку для управления Сервоприводом оси Z Servo Zmotor; // Инициализируем сервопривод оси Z int count; // int count2[3] = {0,0,0}; // int val = 0; // int rot=0; // int incomingByte = 0; // int sign=1; // Переменная направления вращения двигателей осей long delayTime; // Переменная задежки int angle=90; // переменная угла поворота сервомашинки оси Z void setup() { // Задаем параметры работы Zmotor.attach(10); // Указываем, к какому pin'у Arduino подключен сервопривод оси Z Zmotor.write(angle); // Задаем нулевое значение угла для сервопривода int i; // Локальная переменная i (счетсик циклов) Serial.begin(9600); // Инициализация последовательного порта со скоростью 9600 бод delayTime=3000;} // Задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения) /************** Функция движения двигателей ***************/ void moveDir(int sm, int st) { // Функция поворота двигателя с номерм sm на один шаг вперёд в направлении st (-1 или 1) if(sm==0){ // Xmotor #0 if (st==1){Xmotor.step(1);} // Ymotor #1 else {Xmotor.step(-1);}} // Zmotor #2 if(sm==1){ // ****** if (st==1){Ymotor.step(1);} // ***** else {Ymotor.step(-1);}} // **** if(sm==2){ // *** if (st==1){angle++; Zmotor.write(angle);} // ** else {angle--; Zmotor.write(angle);}}} // * /*********** Функция поворота двигателя вперед **********/ void moveForward(int sm){ // Поворот двигателя с номерм sm на один шаг вперёд moveDir(sm,1);} // * /*********** Функция поворота двигателя назад ***********/ void moveBackward(int sm){ // Поворот двигателя с номерм sm на один шаг назад moveDir(sm,-1);} // * /******************* Функция задержки *******************/ 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){ // определение функции и принятых значений х,у,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 if (c[i]<0) { // c2[i]--; // moveBackward(i);}} // if (abs(c1[i]) < abs(c[i])) flg=true;} // delayMicros(delayTime);}} // void loop() { // Цикл основной программы if (Serial.available() > 0) { // Пришла команда long c[4]={0,0,0,0}; // Определение массива координат int i=0; // Опредлеение переменной счетчика циклов sign=1; // Переменная направления вращения (1- по часовой) incomingByte = Serial.read(); // Запись принятых байтов в переменную while (incomingByte!=';') { // Читаем входящую строку, признак конца строки знак "точка с запятой" if (c[i]==0) { // Если символ в c[i] равен 0, то if (incomingByte=='-') sign=-1;} // если он ещё равен сиволу "-", то направление вращения равно -1 (против часовой) if (incomingByte==',') { // Если символ равен ",", то c[i]*=sign; // с[i] умножается на переменную вращения и присваивается снова с[i] sign=1; // переменная направления вращения присваивается 1 i++;} // переменная счетчика циклов увеличивается на 1 else if (incomingByte>='0' && incomingByte<='9') { // Иначе если байт >=0 и <=9, то c[i]=c[i]*10+incomingByte-'0';} // с[i] присваивается с[i] умноженное на 10 и прибавленное входящий байт за вычетом кода 48 ASCII ("0") while (Serial.available() == 0) { // Если равно 0, то delayMicroseconds(1);} // ждём очередной символ 1 микросекунду, вдруг не успел придти incomingByte = Serial.read();} // Запись принятого байта в переменную c[i]*=sign; // с[i] умножается на переменную вращения и присваивается снова с[i] if (c[3]>0) delayTime=c[3]; // Если с[3] больше 0, то задежке между командами (delayTime) присваивается значение с[3] MoveSM(c[0],c[1],c[2]); // Вращаем двигатели на заданное число шагов функцией MoveSM Serial.println("OK");} // Отправляем компьютеру сообщение "OK", это значит можно высылать новую команду else delayMicroseconds(1);} // Если ничего не пришло, ждём 1 микросекуду.Я бы реализовал это так, через подключение библиотек Stepper и Servo. Комментарии сделал для себя, вдруг и Вам пригодятся. Пусть и поздно, но всё же кому-то это может поможет. Тоже начал разбираться в этой теме и наткнулся на этот вопрос, оставленный без ответа.
Не понимаю, в чем тут можно "разбираться" !? Все более менее стандартные контролеры шаговых двигателей имеют всего 2 контакта, направление и шаг. Совершенно так же работает и сервопривод, для контролера это идентичные устройства. Просто берете шаговик с контролером шаговика , выбрасываете его, а на его место ставите контролер сервопривода с сервомотором.
В данном примере было реализовано подлкючение шаговых двигателей из CD-rom'ов через микросхему L293d. Ну или можно через L298N (вроде так маркируется). Собирал из того, что было. Соответственно у такого подключения задействованы 4 пина. Можно конечно сократить и до двух, но через инвертирование вторых выводов управления, а они уже не направление и шаг.
Нужно просто изменить подпрограмму шага в библиотеке (void Stepper::stepMotor(int thisStep)) или использовать другую библиотеку.
Спасибо, буду теперь знать)
Эх....
void Stepper::step(int steps_to_move) { int steps_left = abs(steps_to_move); // how many steps to take // determine direction based on whether steps_to_mode is + or -: if (steps_to_move > 0) { this->direction = 1; } if (steps_to_move < 0) { this->direction = 0; } // decrement the number of steps, moving one step each time: while (steps_left > 0) { unsigned long now = micros(); // move only if the appropriate delay has passed: if (now - this->last_step_time >= this->step_delay) { // get the timeStamp of when you stepped: this->last_step_time = now; // increment or decrement the step number, // depending on direction: if (this->direction == 1) { digitalWrite(motor_pin_1, HIGH); this->step_number++; if (this->step_number == this->number_of_steps) { this->step_number = 0; } } else { digitalWrite(motor_pin_1, LOW); if (this->step_number == 0) { this->step_number = this->number_of_steps; } this->step_number--; } // decrement the steps left: steps_left--; digitalWrite(motor_pin_2, HIGH); } else { digitalWrite(motor_pin_2, LOW); } } }Вот если использовать эту процедуру, то для двухпинового подключения на пин 1 посадить DIR, на пин 2 - STEP, сам не проверял, но имхо должно работать. Четырехпиновое работать не будет вообще. Как это использовать уже подумайте сами.