Advertisement
Guest User

Untitled

a guest
Jul 26th, 2014
203
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 2.23 KB | None | 0 0
  1. boolean linearPos(double lastxpos,double lastypos,double lastzpos,double xpos,double ypos ,double zpos)
  2. {
  3.         // d = delta (steppek)
  4.         long dx = (xpos-lastxpos)/0.0006;
  5.         long dy = (ypos-lastypos)/0.0006;
  6.         long dz = (zpos-lastzpos)/0.0006;
  7.        
  8.         if(dx > 0)  digitalWrite(Xcw, HIGH);        //directions setup
  9.         else  digitalWrite(Xcw, LOW);
  10.         if(dy > 0)  digitalWrite(Ycw, HIGH);
  11.         else  digitalWrite(Ycw, LOW);
  12.         if(dz > 0)  digitalWrite(Zcw, HIGH);
  13.         else  digitalWrite(Zcw, LOW);
  14.        
  15.        
  16.         long res = 100;
  17.         if(abs(dx) >= abs(dy) && abs(dx) >= abs(dz)) res = abs(dx);
  18.         else if(abs(dy) >= abs(dx) && abs(dy) >= abs(dz)) res = abs(dy);
  19.         else if(abs(dz) >= abs(dx) && abs(dz) >= abs(dy)) res = abs(dz);
  20.        
  21.          
  22.         long eddigistepx = 0;
  23.         long eddigistepy = 0;
  24.         long eddigistepz = 0;
  25.        
  26.         // ilyen arányt kell tartani
  27.         double ratex = dx/res;
  28.         double ratey = dy/res;
  29.         double ratez = dz/res;
  30.        
  31.         // mozgassa-e az adott tengelyen
  32.         uint8_t movex = LOW;
  33.         uint8_t movey = LOW;
  34.         uint8_t movez = LOW;
  35.        
  36.         for(long i = 0;i < res;i++)
  37.         {
  38.                 if(eddigistepx/double(i+1) < ratex)
  39.                 {
  40.                         movex = HIGH;
  41.                         eddigistepx++;
  42.                 }
  43.                 if(eddigistepy/double(i+1) < ratey)
  44.                 {
  45.                         movey = HIGH;
  46.                         eddigistepy++;
  47.                 }
  48.                 if(eddigistepz/double(i+1) < ratez)
  49.                 {
  50.                         movez = HIGH;
  51.                         eddigistepz++;
  52.                 }
  53.                
  54.                digitalWrite(Xclk, movex);
  55.                digitalWrite(Yclk, movey);
  56.                digitalWrite(Zclk, movez);
  57.                delayMicroseconds(PULSETIME);
  58.                digitalWrite(Xclk, LOW);
  59.                digitalWrite(Yclk, LOW);
  60.                digitalWrite(Zclk, LOW);
  61.                delayMicroseconds(PULSETIME);
  62.  
  63.                 movex = LOW;
  64.                 movey = LOW;
  65.                 movez = LOW;
  66.         }
  67.        
  68.         return true;
  69. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement