A small CNC

Forum 2005-2010 (read only) General Exhibition
7

Part 2

void DrawXY(){
   //unit on y
      float slope = abs(DX / DY);
      dy = 1;
      if (DY > 0){
        if (DX > 0){
          if (posy < targety){
            motory.step(dy, BACKWARD, SINGLE);
            posy = posy + dy / stepy;
            linex = linex + dy * slope / stepy;
            int dx = (int) ((linex - posx) * stepx);
            motorx.step(dx, BACKWARD, SINGLE);
            posx = posx + dx / stepx;
            report();
          }
        }
        else {
          if (posy < targety){
            motory.step(dy, BACKWARD, SINGLE);
            posy = posy + dy / stepy;
            linex = linex - dy * slope / stepy;
            int dx = (int) (abs(linex - posx) * stepx);
            motorx.step(dx, FORWARD, SINGLE);
            posx = posx - dx / stepx;
            report();
          }
        }
      }
      else{
        if (DX > 0){
          if (posy > targety){
            motory.step(dy, FORWARD, SINGLE);
            posy = posy - dy / stepy;
            linex = linex + dy * slope / stepy;
            int dx = (int) ((linex - posx) * stepx);
            motorx.step(dx, BACKWARD, SINGLE);
            posx = posx + dx / stepx;
            report();
          }
        }
        else {
          if (posy > targety){
            motory.step(dy, FORWARD, SINGLE);
            posy = posy - dy / stepy;
            linex = linex - dy * slope / stepy;
            int dx = (int) (abs(linex - posx) * stepx);
            motorx.step(dx, FORWARD, SINGLE);
            posx = posx - dx / stepx;
            report();
          }
        }
      }
}

void DrawYX(){
  //unit on x
      float slope = abs(DY / DX);
      dx = 1;
      if (DX > 0){
        if (DY > 0){
          if (posx < targetx){
            motorx.step(dx, BACKWARD, SINGLE);
            posx = posx + dx / stepx;
            liney = liney + dx * slope / stepx;
            int dy = (int) ((liney - posy) * stepy);
            motory.step(dy, BACKWARD, SINGLE);
            posy = posy + dy / stepy;
            report();
          }
        }
        else {
          if (posx < targetx){
            motorx.step(dx, BACKWARD, SINGLE);
            posx = posx + dx / stepx;
            liney = liney - dx * slope / stepx;
            int dy = (int) (abs(liney - posy) * stepy);
            motory.step(dy, FORWARD, SINGLE);
            posy = posy - dy / stepy;
            report();
          }
        }
      }
      else{
        if (DY > 0){
          if (posx > targetx){
            motorx.step(dx, FORWARD, SINGLE);
            posx = posx - dx / stepx;
            liney = liney + dx * slope / stepx;
            int dy = (int) ((liney - posy) * stepy);
            motory.step(dy, BACKWARD, SINGLE);
            posy = posy + dy / stepy;
            report();
          }
        }
        else{
          if (posx > targetx){
            motorx.step(dx, FORWARD, SINGLE);
            posx = posx - dx / stepx;
            liney = liney - dx * slope / stepx;
            int dy = (int) (abs(liney - posy) * stepy);
            motory.step(dy, FORWARD, SINGLE);
            posy = posy - dy / stepy;
            report();
          }
        }
      }
}

void report(){
  Serial.print ("X:");
  Serial.println (posx);
  Serial.print("\t");
  Serial.print ("Y:");
  Serial.println (posy); 
  Serial.print("\t");
  Serial.print ("Z:");
  Serial.println (posz); 
  Serial.print ("\n");
}

void CheckArrive(){

  if (abs(targetx-posx) + abs(targety-posy) + abs(targetz-posz)< 0.01 && command == true){
    command = false;
    Serial.print("*");
  }
}

void Reportz(){
  int intposz = (int)(posz * 100);
  mySerial.print("@");
  mySerial.print (intposz);
  mySerial.print("#");
}

void loop() {
  
  AcquireXYZ();
  if (command == true){
    plotting();
  }
  CheckArrive();
}