cnc-club.ru

Код: Выделить всё

void setup() {
  motor1.setSpeed(255);
  motor2.setSpeed(255);
  motor3.setSpeed(255);

 int i;
 Serial.begin(9600);

   attachInterrupt(3, blink1, CHANGE);
   attachInterrupt(2, blink2, CHANGE);
   attachInterrupt(4, blink3, CHANGE);

 delayTime=4; //задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения)
}

void  moveBackward(int sm)
{
  if (sm == 0)
  {
    while (P1 < 1){
    motor1.run(FORWARD);
    }
    motor1.run(RELEASE);
    P1 = 0;
  }
  else if (sm == 1)
  {
    while (P2 < 1){
    motor2.run(FORWARD);
    }
    motor2.run(RELEASE);
    P2 = 0;
  }
  else if (sm == 2)
  {
    while (P3 < 1){
    motor3.run(FORWARD);
    }
    motor3.run(RELEASE);
    motor3.run(BACKWARD);
    delay(5);
    motor3.run(RELEASE);
    P3 = 0;
  }
}

void moveForward(int sm)
{
  if (sm == 0)
  {
    while (P1 < 1){
    motor1.run(BACKWARD);
    }
    motor1.run(RELEASE);
    P1 = 0;
  }
  else if (sm == 1)
  {
    while (P2 < 1){
    motor2.run(BACKWARD);
    }
    motor2.run(RELEASE);
    P2 = 0;
  }
  else if (sm == 2)
  {
    while (P3 < 1){
    motor3.run(BACKWARD);
    }
    motor3.run(RELEASE);
    motor3.run(FORWARD);
    delay(5);
    motor3.run(RELEASE);
    
    P3 = 0;
  }
}

Код: Выделить всё

#include <AFMotor.h>

int motorPins[3][4] = {{2, 3, 4, 5},{7, 8, 6, 9},{10, 11, 12, 13}};
int count;
int count2[3] = {0,0,0};
int delayTime;
int val = 0;
int rot=0;
int incomingByte = 0;
int sign=1;
int P1,P2,P3, s = 0;

AF_DCMotor motor1(3, MOTOR12_64KHZ); 
AF_DCMotor motor2(2, MOTOR12_64KHZ); 
AF_DCMotor motor3(4, MOTOR12_64KHZ); 

void blink1()
{
  P1++;
}

void blink2()
{
  P2++;
}

void blink3()
{
  P3++;
}



//Процедура настройки прошивки
void setup() {
  motor1.setSpeed(255);
  motor2.setSpeed(255);
  motor3.setSpeed(255);

 int i;
 Serial.begin(9600);

   attachInterrupt(3, blink1, CHANGE);
   attachInterrupt(2, blink2, CHANGE);
   attachInterrupt(4, blink3, CHANGE);

 delayTime=4; //задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения)
}

void  moveBackward(int sm)
{
  if (sm == 0)
  {
    while (P1 < 1){
    motor1.run(FORWARD);
    }
    motor1.run(RELEASE);
    P1 = 0;
  }
  else if (sm == 1)
  {
    while (P2 < 1){
    motor2.run(FORWARD);
    }
    motor2.run(RELEASE);
    P2 = 0;
  }
  else if (sm == 2)
  {
    while (P3 < 1){
    motor3.run(FORWARD);
    }
    motor3.run(RELEASE);
    motor3.run(BACKWARD);
    delay(5);
    motor3.run(RELEASE);
    P3 = 0;
  }
}

void moveForward(int sm)
{
  if (sm == 0)
  {
    while (P1 < 1){
    motor1.run(BACKWARD);
    }
    motor1.run(RELEASE);
    P1 = 0;
  }
  else if (sm == 1)
  {
    while (P2 < 1){
    motor2.run(BACKWARD);
    }
    motor2.run(RELEASE);
    P2 = 0;
  }
  else if (sm == 2)
  {
    while (P3 < 1){
    motor3.run(BACKWARD);
    }
    motor3.run(RELEASE);
    motor3.run(FORWARD);
    delay(5);
    motor3.run(RELEASE);
    
    P3 = 0;
  }
}

//Одновременный поворот двигателей 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;
    }
    delay(delayTime);
  }
}

//Основной цикл
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) {
    delay(2);
   }
   incomingByte = Serial.read();
  }
  c[i]*=sign;
  MoveSM(c[0],c[1],c[2]); //Вращаем двигатели на заданное число шагов
  Serial.println("OK");  //Отправляем компьютеру сообщение "OK", значит можно высылать новую команду
 }
 else
  delay(100); //Если ничего не пришло, ждём 1/10 секуды.
}