Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- boolean linearPos(double lastxpos,double lastypos,double lastzpos,double xpos,double ypos ,double zpos)
- {
- // d = delta (steppek)
- long dx = (xpos-lastxpos)/0.0006;
- long dy = (ypos-lastypos)/0.0006;
- long dz = (zpos-lastzpos)/0.0006;
- if(dx > 0) digitalWrite(Xcw, HIGH); //directions setup
- else digitalWrite(Xcw, LOW);
- if(dy > 0) digitalWrite(Ycw, HIGH);
- else digitalWrite(Ycw, LOW);
- if(dz > 0) digitalWrite(Zcw, HIGH);
- else digitalWrite(Zcw, LOW);
- long res = 100;
- if(abs(dx) >= abs(dy) && abs(dx) >= abs(dz)) res = abs(dx);
- else if(abs(dy) >= abs(dx) && abs(dy) >= abs(dz)) res = abs(dy);
- else if(abs(dz) >= abs(dx) && abs(dz) >= abs(dy)) res = abs(dz);
- long eddigistepx = 0;
- long eddigistepy = 0;
- long eddigistepz = 0;
- // ilyen arányt kell tartani
- double ratex = dx/res;
- double ratey = dy/res;
- double ratez = dz/res;
- // mozgassa-e az adott tengelyen
- uint8_t movex = LOW;
- uint8_t movey = LOW;
- uint8_t movez = LOW;
- for(long i = 0;i < res;i++)
- {
- if(eddigistepx/double(i+1) < ratex)
- {
- movex = HIGH;
- eddigistepx++;
- }
- if(eddigistepy/double(i+1) < ratey)
- {
- movey = HIGH;
- eddigistepy++;
- }
- if(eddigistepz/double(i+1) < ratez)
- {
- movez = HIGH;
- eddigistepz++;
- }
- digitalWrite(Xclk, movex);
- digitalWrite(Yclk, movey);
- digitalWrite(Zclk, movez);
- delayMicroseconds(PULSETIME);
- digitalWrite(Xclk, LOW);
- digitalWrite(Yclk, LOW);
- digitalWrite(Zclk, LOW);
- delayMicroseconds(PULSETIME);
- movex = LOW;
- movey = LOW;
- movez = LOW;
- }
- return true;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement