Нужна помощь в переделке кода с оптик энкодерами

Войдите на сайт для отправки комментариев

6 ответов [Последнее сообщение]

Вс, 26/03/2017 - 19:47

Wikly

Offline

Зарегистрирован: 20.01.2015

Всем доброго времени суток.

Изобретаю станок ЧПУ с DC моторами и оптическими энкодерами.

имеется такой код:

//CNC router
//
//Code to run on the command computer see:
//commands-plotter.c
//
//Source & info: www.HomoFaciens.de


//#include <avr/io.h>
//#include <util/delay.h>
#include <Servo.h> 

//Change values below to adapt your motor

#define STEP_MARGIN 1L       //10 - 1000 (2)
#define MIN_DUTYCYCLE 120     //0 - 255 (125)
#define MAX_DUTYCYCLE 255    //0 - 255 (255)

#define MX_S1 A0             //Sensor input 1
#define MX_S2 A1             //Sensor input 2
#define MX_DIR 2             //Direction output motor
#define MX_PWM 3             //PWM output motor

#define MY_S1 A2             //Sensor input 1
#define MY_S2 A3             //Sensor input 2
#define MY_DIR 4             //Direction output motor
#define MY_PWM 5             //PWM output motor

#define MZ_S1 A4             //Sensor input 1
#define MZ_S2 A5             //Sensor input 2
#define MZ_DIR 7             //Direction output motor
#define MZ_PWM 6             //PWM output motor


#define RELAY  9          //Relay used to turn on milling machine


int MxSensorRead2 = 0;
int MxSensorRead1 = 0;
int MxDutyCycle = 100; // 10 - 255
unsigned long MxSetPoint = 0;
unsigned long MxActualPoint = 0;
byte MxStep = 0;
byte MxStepDone = 0;

int MySensorRead2 = 0;
int MySensorRead1 = 0;
int MyDutyCycle = 100; // 10 - 255
unsigned long MySetPoint = 0;
unsigned long MyActualPoint = 0;
byte MyStep = 0;
byte MyStepDone = 0;

int MzSensorRead2 = 0;
int MzSensorRead1 = 0;
int MzDutyCycle = 100; // 10 - 255
unsigned long MzSetPoint = 0;
unsigned long MzActualPoint = 0;
byte MzStep = 0;
byte MzStepDone = 0;

byte readByte = 0;


unsigned long stepTime = 1;

unsigned long millisStart = 0;


void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('X');   // send a capital X
    delay(300);
  }
}

void printValues() {
  Serial.print(MxActualPoint);
  Serial.print(", ");
  Serial.print(MxSetPoint);
  Serial.print(", ");
  Serial.print(MxStep);
}

void setup(){ 

  pinMode(RELAY, OUTPUT);
  digitalWrite(RELAY, 0);
  

  pinMode(MX_DIR, OUTPUT);     
  pinMode(MX_PWM, OUTPUT);     
  analogWrite(MX_PWM, 0);
  digitalWrite(MX_DIR, 0);
  
  pinMode(MX_S1, INPUT);
  pinMode(MX_S2, INPUT);
  
  
  pinMode(MY_DIR, OUTPUT);     
  pinMode(MY_PWM, OUTPUT);     
  analogWrite(MY_PWM, 0);
  digitalWrite(MY_DIR, 0);
  
  pinMode(MY_S1, INPUT);
  pinMode(MY_S2, INPUT);


  pinMode(MZ_DIR, OUTPUT);     
  pinMode(MZ_PWM, OUTPUT);     
  analogWrite(MZ_PWM, 0);
  digitalWrite(MZ_DIR, 0);
  
  pinMode(MZ_S1, INPUT);
  pinMode(MZ_S2, INPUT);

  
  stepTime = 1;
  readByte = 0;
  
  MxStep = 0;
  MyStep = 0;
  MzStep = 0;

  MxStepDone = 1;
  MyStepDone = 1;
  MzStepDone = 1;
  
  MxSetPoint     = 300000;
  MxActualPoint  = 300000;

  MySetPoint     = 300000;
  MyActualPoint  = 300000;

  MzSetPoint     = 300000;
  MzActualPoint  = 300000;

  // start serial port at 115200 bps:
  Serial.begin(115200);
  establishContact();  // send a byte to establish contact until receiver responds 
}


void loop(){  

  millisStart = millis();
  while(millis() - millisStart < stepTime){
    do{
      MzSensorRead1 = digitalRead(MZ_S1);
      MzSensorRead2 = digitalRead(MZ_S2);
    
      if(MzSensorRead2 == 0 && MzSensorRead1 == 0){
        if(MzStep == 3){
          MzActualPoint++;
        }
        if(MzStep == 1){
          MzActualPoint--;
        }
        MzStep = 0;
      }
    
      if(MzSensorRead2 == 1 && MzSensorRead1 == 0){
        if(MzStep == 0){
          MzActualPoint++;
        }
        if(MzStep == 2){
          MzActualPoint--;
        }
        MzStep = 1;
      }
    
      if(MzSensorRead2 == 1 && MzSensorRead1 == 1){
        if(MzStep == 1){
          MzActualPoint++;
        }
        if(MzStep == 3){
          MzActualPoint--;
        }
        MzStep = 2;
      }
    
      if(MzSensorRead2 == 0 && MzSensorRead1 == 1){
        if(MzStep == 2){
          MzActualPoint++;
        }
        if(MzStep == 0){
          MzActualPoint--;
        }
        MzStep = 3;
      }

      if(abs(MzSetPoint - MzActualPoint) < STEP_MARGIN){
        analogWrite(MZ_PWM, 0);
        digitalWrite(MZ_DIR, 0);
        MzStepDone = 1;
        MzDutyCycle = MIN_DUTYCYCLE;
      }
      else{
        if(MzActualPoint < MzSetPoint){
          digitalWrite(MZ_DIR, 1);
          analogWrite(MZ_PWM, 255 - MzDutyCycle);
        }
        if(MzActualPoint > MzSetPoint){
          digitalWrite(MZ_DIR, 0);
          analogWrite(MZ_PWM, MzDutyCycle);
        }
      }

      MySensorRead1 = digitalRead(MY_S1);
      MySensorRead2 = digitalRead(MY_S2);
    
      if(MySensorRead2 == 0 && MySensorRead1 == 0){
        if(MyStep == 3){
          MyActualPoint++;
        }
        if(MyStep == 1){
          MyActualPoint--;
        }
        MyStep = 0;
      }
    
      if(MySensorRead2 == 1 && MySensorRead1 == 0){
        if(MyStep == 0){
          MyActualPoint++;
        }
        if(MyStep == 2){
          MyActualPoint--;
        }
        MyStep = 1;
      }
    
      if(MySensorRead2 == 1 && MySensorRead1 == 1){
        if(MyStep == 1){
          MyActualPoint++;
        }
        if(MyStep == 3){
          MyActualPoint--;
        }
        MyStep = 2;
      }
    
      if(MySensorRead2 == 0 && MySensorRead1 == 1){
        if(MyStep == 2){
          MyActualPoint++;
        }
        if(MyStep == 0){
          MyActualPoint--;
        }
        MyStep = 3;
      }

      if(abs(MySetPoint - MyActualPoint) < STEP_MARGIN){
        analogWrite(MY_PWM, 0);
        digitalWrite(MY_DIR, 0);
        MyStepDone = 1;
        MyDutyCycle = MIN_DUTYCYCLE;
      }
      else{
        if(MyActualPoint < MySetPoint){
          digitalWrite(MY_DIR, 1);
          analogWrite(MY_PWM, 255 - MyDutyCycle);
        }
        if(MyActualPoint > MySetPoint){
          digitalWrite(MY_DIR, 0);
          analogWrite(MY_PWM, MyDutyCycle);
        }
      }





      MxSensorRead1 = digitalRead(MX_S1);
      MxSensorRead2 = digitalRead(MX_S2);
    
      if(MxSensorRead2 == 0 && MxSensorRead1 == 0){
        if(MxStep == 3){
          MxActualPoint++;
          printValues();
        }
        if(MxStep == 1){
          MxActualPoint--;
          printValues();
        }
        MxStep = 0;
      }
    
      if(MxSensorRead2 == 1 && MxSensorRead1 == 0){
        if(MxStep == 0){
          MxActualPoint++;
          printValues();
        }
        if(MxStep == 2){
          MxActualPoint--;
          printValues();
        }
        MxStep = 1;
      }
    
      if(MxSensorRead2 == 1 && MxSensorRead1 == 1){
        if(MxStep == 1){
          MxActualPoint++;
          printValues();
        }
        if(MxStep == 3){
          MxActualPoint--;
          printValues();
        }
        MxStep = 2;
      }
    
      if(MxSensorRead2 == 0 && MxSensorRead1 == 1){
        if(MxStep == 2){
          MxActualPoint++;
          printValues();
        }
        if(MxStep == 0){
          MxActualPoint--;
          printValues();
        }
        MxStep = 3;
      }

      if(abs(MxSetPoint - MxActualPoint) < STEP_MARGIN){
        analogWrite(MX_PWM, 0);
        digitalWrite(MX_DIR, 0);
        MxStepDone = 1;
        MxDutyCycle = MIN_DUTYCYCLE;
      }
      else{
        if(MxActualPoint < MxSetPoint){
          digitalWrite(MX_DIR, 1);
          analogWrite(MX_PWM, 255 - MxDutyCycle);
        }
        if(MxActualPoint > MxSetPoint){
          digitalWrite(MX_DIR, 0);
          analogWrite(MX_PWM, MxDutyCycle);
        }
      }



      
    }while(MzStepDone == 0 || MyStepDone == 0 || MxStepDone == 0 );
    if(readByte == 0){
      millisStart = 0L;
    }
  }//while(millis() - millisStart < stepTime){
  
  readByte = 0;
  if (Serial.available() > 0){//if we get a valid byte, read from serial:
    //get incoming byte:
    readByte = Serial.read();
    Serial.print('r');   // send a 'r' to initiate next data from computer
  }//if (Serial.available() > 0){


  
  if(readByte > 0){

    if(readByte == 'a'){
      MxSetPoint++;
      MxDutyCycle = MAX_DUTYCYCLE;
      MxStepDone = 0;
    }
    if(readByte == 'b'){
      MxSetPoint--;
      MxDutyCycle = MAX_DUTYCYCLE;
      MxStepDone = 0;
    }

    if(readByte == 'c'){
      MySetPoint++;
      MyDutyCycle = MAX_DUTYCYCLE;
      MyStepDone = 0;
    }
    if(readByte == 'd'){
      MySetPoint--;
      MyDutyCycle = MAX_DUTYCYCLE;
      MyStepDone = 0;
    }

    if(readByte == 'e'){
      MzSetPoint++;
      MzDutyCycle = MAX_DUTYCYCLE;
      MzStepDone = 0;
    }
    if(readByte == 'f'){
      MzSetPoint--;
      MzDutyCycle = MAX_DUTYCYCLE;
      MzStepDone = 0;
    }
      
    if(readByte == 'R'){
      digitalWrite(RELAY, 1);
      readByte = 0;
    }
    if(readByte == 'r'){
      digitalWrite(RELAY, 0);
      readByte = 0;
    }

    if(readByte == 'S'){
      if(stepTime < 1000){
        stepTime+=10;
      }
    }
    if(readByte == 's'){
      if(stepTime > 10){
        stepTime-=10;
      }
    }
  }//if(readByte > 0){

}

В этом коде идет работа со сдвоенными Оптическими энкодерами, хочу переделать для работы с одинарными, типа такого ( http://www.conrad.com/ce/en/product/184250/Vishay-TCST-2103-CNY-37-Photoelectric-Fork-Sensor )

обработка сигналов идет тут:

 MzSensorRead1 = digitalRead(MZ_S1);
      MzSensorRead2 = digitalRead(MZ_S2);
    
      if(MzSensorRead2 == 0 && MzSensorRead1 == 0){
        if(MzStep == 3){
          MzActualPoint++;
        }
        if(MzStep == 1){
          MzActualPoint--;
        }
        MzStep = 0;
      }
    
      if(MzSensorRead2 == 1 && MzSensorRead1 == 0){
        if(MzStep == 0){
          MzActualPoint++;
        }
        if(MzStep == 2){
          MzActualPoint--;
        }
        MzStep = 1;
      }
    
      if(MzSensorRead2 == 1 && MzSensorRead1 == 1){
        if(MzStep == 1){
          MzActualPoint++;
        }
        if(MzStep == 3){
          MzActualPoint--;
        }
        MzStep = 2;
      }
    
      if(MzSensorRead2 == 0 && MzSensorRead1 == 1){
        if(MzStep == 2){
          MzActualPoint++;
        }
        if(MzStep == 0){
          MzActualPoint--;
        }
        MzStep = 3;
      }

      if(abs(MzSetPoint - MzActualPoint) < STEP_MARGIN){
        analogWrite(MZ_PWM, 0);
        digitalWrite(MZ_DIR, 0);
        MzStepDone = 1;
        MzDutyCycle = MIN_DUTYCYCLE;
      }
      else{
        if(MzActualPoint < MzSetPoint){
          digitalWrite(MZ_DIR, 1);
          analogWrite(MZ_PWM, 255 - MzDutyCycle);
        }
        if(MzActualPoint > MzSetPoint){
          digitalWrite(MZ_DIR, 0);
          analogWrite(MZ_PWM, MzDutyCycle);
        }
      }

Подскажите как можно переписать код для ардуино, чтоб шаги считались с одинарными энкодерами? Разумеется без учета направления?...

Заранее всем откликнувшимся - спасибо!!!

Вс, 26/03/2017 - 20:06

Wikly

Offline

Зарегистрирован: 20.01.2015

Сам станок уже гоов, и работает: http://www.cnc-club.ru/forum/viewtopic.php?f=40&t=15148&p=347947#p347947

Но не нравится управляющая программа.. Потому и хотелось бы переделать. Но к сожалению, достать нужные энкодеры, нет возможности.

Вс, 26/03/2017 - 20:07

ЕвгенийП

Offline

Зарегистрирован: 25.05.2015

Этот код Вы сами писали? Или скачали где-нибудь?

Вс, 26/03/2017 - 20:17

Wikly

Offline

Зарегистрирован: 20.01.2015

Код взят с сайта: http://www.homofaciens.de/technics-machines-cnc-v2_en.htm

Сейчас станок работает с проекта ecnc.ru, но код переписал для работы с DC моторами и энкодерами (одинарными)

#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 секуды.
}

Вс, 26/03/2017 - 20:22

ЕвгенийП

Offline

Зарегистрирован: 25.05.2015

Т.е. Вы скачали код, а сейчас хотите его модифицировать, правильно?

Лучше всего Вам обратиться к автору кода. Если такой возможности нет, то попробуйте обратиться в раздел "Ищу исполнителя". не думаю, что что в этом разделе Вам помогут. Всё может быть, но вероятность невелика.

Вс, 26/03/2017 - 20:27

Wikly

Offline

Зарегистрирован: 20.01.2015

Правильно. К автору обратится нет возможности.

Знаю что можно модифицировать код, но уже всю голову сломал. запутался в алгоритме работы данного исходника. вот и подумал, может кто свежим взглядом гланет, да и поможет. :)

Вс, 26/03/2017 - 21:01

ЕвгенийП

Offline

Зарегистрирован: 25.05.2015

Ну, попробуйте в "Ищу исполнителя"

Arduino.ru

Регистрация новых пользователей и создание новых тем теперь только на новом форуме https://forum.arduino.ru

Нужна помощь в переделке кода с оптик энкодерами