Modify repetier firmware for SCARA

edited September 2016 in Questions & Answers
Dear repetier,

First of all i want to thanks for the great job on repetier firmware and host. I love it much more than marlin.

I'm using repetier firmware for my delta printer, and now i'm trying build my own design scara arm 3d printer. So i'm trying to modify repetier firmware.

Hopefully it's okay and i get some guidance.

What i've done is add new drive system called scara.
Then i add some new functions for converting.
First function is scara inverse kinematics, this is to convert cartesian coordinate to scara.
Second function is scara forward kinematics, this is to convert scara coordinate to cartesian.
Third function is dummy convert cartesian to delta. This is because i include scara drive to non linear, and i see there are some functions use the function.

The main idea is cartesian use x & y axis linearly, i use x & y axis circulary. While z axis is the same as cartesian.
So cartesian step per unit is step per mm, scara step per unit is step per degree.

The main different converting cartesian to scara compare to convert to delta is which one to convert first.
i see that repetier firmware convert mm to steps then convert to delta towers in steps.
While scara have to convert coordinate in mm to coordinate in degree first then convert in steps.

That's why i try to convert all the command in cartesian to scara in units then convert to steps.

My questions are
1. What functions that i have to modify.
I add conversion scara-cartesian in motion.cpp, I found ispositionallowed(), updatederivedparameter(), moveto(), movetoreal(), setdestinationstepsfromgcode() in printer.cpp that i've modify, do i miss something?.

2. I'm confused with moveto() and moveto() function.
A. I read the code and the comments but i dont really understand what is the different. Can you please explain what is the meaning of untransformed move, transformed cartesian coordinate, absolute cartesian space, mapping real(model) space to printer space.?
B. In moveto() function. If i'm not mistaken, It's written if value is not ignored, then destinationsteps = (value + offset) * steppermm.
What happen if it's ignored, what is the value of destinationsteps?
I mean if moveto(5, ignore, ignore, ignore)
I believe the x value is 5, what is the value of y?
Because when convert cartesian to scara it need both x & y coordinate to get how much steps x & y motors should go.

3. Questions about homing.
A. My scara have 2 parameters, armlength & forearmlength. X motor to move arm, y motor to move forearm.
The ideal home coordinate is x motor at 0 degree and y motor at 180 degree. But it is not the fact. Let say x motor at 5, y motor at 175. What is this suppose to call? X offset & y offset or must be something else?

B. I set my print bed at the left side of the center axis (x motor). Because of x & y motor home coordinate is not ideal, it's forming a distance. I call it scara minimum radius, the maximum radius is armlength + forearmlength. In between min & max radius is my print bed area. Let say min radius is 50mm, max radius is 250mm and i want to put my print bed at leftside. So my printbed center is at (-150,0) cartesian position from x motor axis.
Which one suppose to be the (0,0)? The x motor axis, nozzle position when touch endstop, center of printbed, or somewhere else?
I am thinking it suppose to be the x motor axis, but i'm thinking how to use repetier host & cura about the printer shape.

That's all for now, hopefully it's too many questions & i hope you can kindly guide me.

Best regards


  • Sounds great. Hope you work against dev tree and make a pull request once you are done!

    Implementing new systems should be quite simple in repetier. First select new drive system as you did and check it is recogniced as NONLINEAR_SYSTEM, which is what you need. You need only to program homing and your own version for 

    uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t corePosSteps[])

    and you are done. First you should understand how to handle steps right. User enters real coordinates (currentPosition). If you check for example moveTo Real the main logic is then

        transformToPrinter(x + Printer::offsetX, y + Printer::offsetY, z + Printer::offsetZ, x, y, z);

        // There was conflicting use of IGNOR_COORDINATE

        destinationSteps[X_AXIS] = static_cast<int32_t>(floor(x * axisStepsPerMM[X_AXIS] + 0.5f));

        destinationSteps[Y_AXIS] = static_cast<int32_t>(floor(y * axisStepsPerMM[Y_AXIS] + 0.5f));

        destinationSteps[Z_AXIS] = static_cast<int32_t>(floor(z * axisStepsPerMM[Z_AXIS] + 0.5f));

    You see it gets transformed (meaning rotation of bed) and then converted to steps. So currentPositionSteps and destinationSteps are in cartesian printer format with steps per mm resolution. Your problem is X and Y which has nothing to do with rotation. So select for x and y a virtual resolution. So say you take 1000 for both. Using axisStepsPerMM you can convert this back into floats if needed or you keep computing with integers if that is faster. Next you implement 

    uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t deltaPosSteps[])

    input is cartesianPosSteps = destination coordinates in steps as described

    output deltaPosSteps = real motor position, so z stays the same and x and y are A/B rotations. You need to introduce a new parameter for your real motor resolution to do the conversion.

    Last you need to write your own homing routine (see printer.cpp how we already have 3 implementations for different drive systems). There are lots of delta parameter in eeprom you can misuse for your system, e.g. setDeltaTowerXOffsetSteps for homing offsets. Please update eeprom input/output so it shows user correct names for scara then.

    You might need to make some assumptions how bed coordinate is orientated relative to arm. If we assume arm is to right a natural coordinate system would have xmin/ymin on the left front. Or you take bed center as origin, both would work.

    Below some doc I wrote for orientation. I hope this covers your first questions. Ask again if you need more help.

    There are 4 coordinate systems:

    Printer::currentPosition - Extruder position in float real word coordinates 
    relative to printer origin. Printer::coordinateOffset is never included and only
    added when M114 is shown or subtracted when coordinates are send.

    On this coordinates we always add Printer::offsetX and Printer::offsetY during operations.

    When bedleveling is active, these 3d coordinates get transformed to bed coordinate system.

    Multiplying with steps per mm yields the
    destionationSteps coordinates in R3

    For nonlinear motions a last transformation maps moves an axis:
    transform caresian step position to nonlinear coordinate string.

    coordinateOffset get subtracted from coordinates given to get the printer x,y,z
    Example: At Position 15 a G92 X40 is given. coordinateOffsetX = 25
    G1 X40 give 40-25 = 15 as real coordinate. 

    Helper function:

    void Printer::updateCurrentPosition()

      Sets Printer::currentPosition based on Printer::currentPositionSteps
      considering offsetX/Y and coordinateOffset
    static inline void Printer::realPosition(float &xp,float &yp,float &zp)
      fills xp,yp,zp with Printer::currentPosition
    void Printer::moveTo(float x,float y,float z,float e,float f)
      Moves printer to x,y,z coordinates neglecting active bed transformations
      and coordinate shift (G92)
    void Printer::moveToReal(float x,float y,float z,float e,float f)
      Moves printer to real coordinates including all transformations
    void PrintLine::moveRelativeDistanceInSteps(long x,long y,long z,long e,float feedrate,bool waitEnd,bool checkEndstop)
      - No range check
      - no coordinate transformation
      - distance in steps
      - updates currentPosition, but not lastCmdPos !
    void PrintLine::moveRelativeDistanceInStepsReal(long x,long y,long z,long e,float feedrate,bool waitEnd)
      - Range check
      - coordinate transformation
      - distance in steps
    uint8_t transformCartesianStepsToDeltaSteps(long cartesianPosSteps[], long deltaPosSteps[])
      transform caresian step position to nonlinear coordinate strin
  • Hi repetier, thx for your kind reply.

    I still confuse about coordinate transformation.
    I checked this function  transformToPrinter(x + Printer::offsetX, y + Printer::offsetY, z + Printer::offsetZ, x, y, z); 
    and confuse about what it does. is it simply the same as
    x = x+offsetX;
    y = y+offsetY;
    z = z+offsetZ: ???

    for coordinate system... this is the map that I think fit to my case

    -------------------------     E
    | |       \
    | |        \O2
    | |        /
    | | =O1/ >y
    | |
    | |
    | |

    in the map, the square is the print bed, O1 is arm shoulder (X motor position), O2 is arm elbow (Y motor position), E is extruder position,
    / is arm length, \ is forearm length, ^x is x positive, >y is y positive, = is minimum distance allowed between shoulder and bed.
    I want O1 to be the (0,0) position.

    1. let say if I want extruder move to (-10,10) at this coordinate system. which function should I use? moveTo(-10,10,ignore) or moveToReal (-10,10,ignore). what is the difference result if I use moveTo and moveToReal in this case?

    2. the bed is at the left side of (0,0), I set the farest bed side from (0,0) is (-50,0), the nearest side of bed is (-10,0), so the center of bed is (-30,0). Do I need OffsetX, OffsetY in this coordinate system? 

    3. I want shoulder(X motor) as the center of coordinate as map, not the bed center, because inverse kinematics equation that I use base on this mapping. if user give (X,Y) cartesian coordinate, I have to transform it to scara coordinate with this equation.
    B = acos ((X^2+Y^2-L^2-M^2)/(2*L*M))
    A = atan (Y/X) - atan (M*sinB/(L+M*cosB)
    A is X motor scara coordinate in rad, B is Y motor scara coordinate.
    as you can see the equation need both X and Y cartesian coordinate to get successful transformation.

    I see in moveToReal, for example...
            x = currentPosition[X_AXIS];
    I understand that if user command moveToReal(Ignore, -20, ignore, ignore), than the x value will be currenPosition[X_Axis]
    so if I transform the given coordinate, it would be ( currentPosition[X_Axis] , -20)

    but I don't understand moveTo function, it doesn't say anything about if ignored.
    I mean if user command moveTo(ignore, -20, ignore, ignore), what is the value of x if the coordinate have to be transformed to scara with the equation above?

    btw, I make changes log for the modification that I made. because I am not really a programmer, can you please help me to check my work. I don't know am I doing the right thing. 

  • transformToPrinter switches from ideal coordinates to coordinates corrected by rotation.

    moveTo does not use the transformation while movetoReal includes transformations, so what users use at the end is moveToReal while for homing you will use moveTo as you want to ignore transformations there.

    Making O1 origin is a bad idea. Then 0,0 would be outside bed and all Y coordinates are negative. That is very counter intuitive. You axis are wrong. We use a right handed coordinate system. The way you have drawn it all objects will be mirrored. Switch x and y and it looks good. Make x = 0 = left side. y = 0 could be middle to give you some advantage on transformation. Modifying coordinate system for your computations is a simple addition, so don't center too much on what's best for your formula, think more from usage point.

    Don't think too much about coordinate systems. You only transform in transformCartesianStepsToDeltaSteps and there you get positions in steps as entered for x/y. All transformations are already done so you only do your simple math. So add offset to xy and compute A/B. 

    I hope you are using the due as your math is very slow. Don't use power 2 - use x*x which is much faster. asin/acos/atan are among the most expensive math computations. If you can replace equations with sqrt instead do it.
  • Tx for your explanations and advices.
    I will avoid using pow ect. But scara arm units conversion to steps options are only degree and rad. To convert angle to linear as far as i know only use sin cos tan asin acos atan functions. I have no idea how to replace those functions.

    From your explanation, i think i can't use moveto() function. Even if i have to use them, the x and y in moveto() must not treated for cartesian coordinate. I mean i have to use them for scara coordinate directly. so for instance moveto(5, ignore) means x motor rotate 5 unit, y motor doesnt move.
    Is this right?

    Sorry... you are right for the map. I mean ^y, >x. And all x coordinates are negative.
    And you are right that it will be not intuitive.

    For the bed, actually the maximum usage is not square nor cicle. It's half circle, but it seems there is no such bed shape provided in repetier host, so the option is only square or circle. Which one you suggest will be better? Square or circle?

    Please correct me, if the origin is bed center. Bed center is (0,0) and O1 let say (30,0).
    If i insist O1 is (0,0) then bed center is (-30,0) and this is not intuitive for user.
    To make bed is origin, simply use offset... x offset?
    I confuse x offset should be 30 or -30 in this case ???

  • moveTo is always cartesian. You should not think about that too much. Your job is to make the right transformations. Most problematic will be homing where you want to set target motor position directly in your polar coordinates I guess. I think you need a new inject method for this - in your case a simplified "queueNonlinearMove" function that has no transformation.

    DOn't think about the offsets we already have. These are for G92 and do change the coordinate system the user uses but not the resulting coordinates. Just add a variable for this on your own taken from delta variables, e.g. diagonal correction variables for x offset, Arm a/b length. Then you can correct in eeprom.

    Bed shape is what you put on your printer not what you can reach. So I'd take a square bed and position it such that you can reach all positions. Scara has a big working radius and I know it is more a circle around arm. So if you want extreme take a circular bed and don't use the center:-)

    Origin of coordinate system is also rotation point. This is important if you start autoleveling bed. It's always nice to have a point that does not change height due to rotation. Math will cover different origin, but it could result that all bed position have negative/positive correction which does not feel so good. SO many things are more like how would users expect things and not what is the mathematical simplest solution.
  • I do worry about moveto(), because i see it used many times (i haven't check them all) and it might causing problems.
    I dont know what will happen to atan(y/x) if y or x is ignored

    So... you are suggesting to use O1 to be origin(0,0), but to make comfortable for users, make a dummy origin that intuitive, then all dummy coordinate transformed/corrected relative to O1?
    Isn't it the same as offset?
    Why not just use x offset then in user languages call it correction?

    For homing i'm not worry too much it's less difficult then delta, and not so much different with cartesian. I don't need to do anything for z. Just transform xy linear to scara.
    The only problem that don't know what to do is i want it home z first, then y, then x.

    Autobed leveling is important to me. That is one of the thing i love about repetier firmware.
    I design this scara bot for open bed, which means users can modify bed according to their applications.
    They may directly print on a floor, a glass, a plate, a wood, a metal or anything. I don't give any boundary to limit the bed, it even possible to print if you put the machine on a table then print on floor. And it even possible to modify to print to wall. So autoleveling is a must everytime user changing bed.

    Autolevel is something that i worry too. Is it use moveto function? If you say moveto must be cartesian, then i can't use it. I guess i must make changes to all commands that use moveto().

    Btw, this is just an information that i hope useful for you. I use repetier firmware for my delta printer.
    I learned about delta for months to get a proper way to calibrate. I search anywhere to get it but havent found any calibration procedure that satisfy me. So i make my own calibrating steps and succesfully get accurate result. Including how to find accurate diagonal & radius for each tower.

    For calibrating delta accurately, calculating steps from a point at any towers to it's endstop is crusial. We can't get accurate diagonal & radius without this.

    I know there is a command in repetier firmware gcode to do this, i was use it and found that it is not correct for some points (especially if we move extruder near z tower, we'll get strange behaviour).
    Try to check distance in steps for coordinate (0,90), (0,85), (10, 80), (-10,80). It happens to my printers that one of the value doesnt change for all that points.

    I found it because transformcartesiantodelta() and step limits in the calculation. So i make another gcode, another function and another integers without transformation and limits to fix it and get accurate result.
    I hope you add a gcode to calculate steps to endstop without transformcartesiantodelta and limits in the next version.

  • Hi, repetier.

    I modified distortion:: in printer.cpp
    It map distortion in steps, I add elif drive_system == scara that map distortions in mm, but when store to eeprom i transform it to scara coordinate.
    When call from eeprom, i transform it again to cartesian in mm.

    I haven't change anything in bedleveling.cpp, but I've checked and found moveto() used many times.

    What if i add elif drive_system scara in moveto(... if x ignored, x =lastcmdpos x axis ?
    Is this solving my problem or it will causing other problems?

  • You are not supposed to change moveTo!!! It is a helper function and takes coordinates in cartesian coordinates and all callers expect it to be using cartesian coordinates. It is cartesian without applying bed leveling correction.

    Same with distortion map. These corrections are applied in cartesian coordinate system and need no special case. I didn't need it for delta so you never need it. In delta we also have cartesian and delta coordinates and the only place we transform is transformcartesiantodelta. It is really that simple.

    Regarding x offset. The one I mean is the one set by G92 and homing resets this to machine origin which is the only origin that counts. And we want this on bed so we can not use it.
  • Hi repetier, tx for your explanation.

    I think I did it all wrong for cartesian transformation.
    I was use steps per mm to be treated as steps per degree, so I modify all functions that convert position to steps. 
    please correct me if this one is wrong... 
    what I suppose to do is make a dummy stepsPerMM for x & y axis. let say define it 1000. 
    create new variables let say stepsPerRad[] for x & y.
    in transformCartesianToDelta, I multiply armLength and forearmLength with 1000 so it will equal with cartesian steps. 
    then convert it to scara coordinate with stepsPerRad[].?

  • No new variables needed. transformCartesianToDelta gets cartesian steps per mm and the result array needs steps from home position of motor. So x/y are your rads multiplied with steps per rad.

    float x = in[X_AXIS]*axisStepsPerMM[X_AXIS] + X_OFFSET;
    float y = in[Y_AXIS]*axisStepsPerMM[Y_AYIS]
    out[Z_AXIS] = in[Z_AXIS]
    out[X_AXIS] = your transform result for x motor steps
    out[Y_AXIS] = your transform result for y motor steps

    no guarantee for variables, just what your transform would look like.

  • Is this correct?

    uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t scaraPosSteps[])
    int32_t tempX = cartesianPosSteps[X_AXIS] + Printer::BedOriginPos[X_AXIS] * axisStepsPerMM[X_AXIS];
    int32_t tempY = cartesianPosSteps[Y_AXIS];
    int32_t temp1 = tempX * tempX + tempY * tempY;
    int32_t tempL = EEPROM::armLength() * 1000; // Arm Length
    int32_t tempM = EEPROM::forearmLength() * 1000; // Forearm Length
    temp1 = tempX * tempX + tempY * tempY - tempL * tempL - tempM * tempM ; // X^2 + Y^2 - L^2 - M^2
    int32_t temp2 = 2 * tempL * tempM ; // 2 * L * M
    float tempB = acos( temp1 / temp2 ); // Elbow angle position
    if (tempB <0) tempB = tempB + 3.1415926535; // Make Elbow angle always positive
    if (tempX == 0) tempX = 0.000000001;
    temp1 = atan ( tempY / tempX ); // atan (Y/X)
    if (temp1<0) temp1 = temp1 + 3.1415926535; // Make the angle always positive
    float temp3 = tempL + tempM * cos( tempB);
    if (temp3 == 0) temp3 = 0.000000001;
    temp2 = atan ( tempM * sin( tempB ) / temp3); // atan (M * sin (Elbow) / (L + M cos (Elbow)))
    if (temp2<0) temp2 = temp2 + 3.1415926535; // Make the angle always positive
    scaraPosSteps[X_AXIS] = (temp1 - temp2) * Printer::motorStepsPerRad[X_Axis]; // Shoulder angle position
    scaraPosSteps[Y_AXIS] = tempB * Printer::motorStepsPerRad[Y_Axis];
    scaraPosSteps[Z_AXIS] = cartesianPosSteps[Z_AXIS];
    return 1;
  • edited September 2016
    void Printer::ScaraMoveToEndstop()
        for (fast8_t i = 0; i < 3; i++)
            Printer::currentPositionSteps[i] = 0;
    Printer::stepsRemainingAtXHit = -1;
        Printer::stepsRemainingAtYHit = -1;
        Printer::stepsRemainingAtZHit = -1;
        PrintLine::moveRelativeDistanceInSteps(0, 0, scaraHomePosSteps[Z_AXIS] * 2, 0, Printer::homingFeedrate[Z_AXIS], true, true);
        if (Endstops::zMax())
    PrintLine::moveRelativeDistanceInSteps(0, Printer::yMotorMaxAngleSteps * 2, 0, 0, Printer::homingFeedrate[Y_AXIS], true, true);
        if (Endstops::yMax())
    PrintLine::moveRelativeDistanceInSteps(- Printer::xMotorMaxAngleSteps * 2, 0, 0, 0, Printer::homingFeedrate[X_AXIS], true, true);
    if (Endstops::xMin()) 
    offsetX = offsetY = offsetZ = 0;

    void Printer::homeXAxis()

    void Printer::homeYAxis()

    void Printer::homeZAxis()
        long steps;
    coordinateOffset[Z_AXIS] = 0;
            steps = (zMaxSteps - zMinSteps) * Z_HOME_DIR;
            currentPositionSteps[Z_AXIS] = -steps;
            PrintLine::moveRelativeDistanceInSteps(0, 0, 2 * steps,0,homingFeedrate[Z_AXIS],true,true);
            currentPositionSteps[Z_AXIS] = zMaxSteps;
            PrintLine::moveRelativeDistanceInSteps(0, 0, axisStepsPerMM[Z_AXIS] * -ENDSTOP_Z_BACK_MOVE * Z_HOME_DIR, 0, homingFeedrate[Z_AXIS] / ENDSTOP_Z_RETEST_REDUCTION_FACTOR,true,false);
            PrintLine::moveRelativeDistanceInSteps(0,0,axisStepsPerMM[Z_AXIS] * 2 * ENDSTOP_Z_BACK_MOVE * Z_HOME_DIR, 0, homingFeedrate[Z_AXIS] / ENDSTOP_Z_RETEST_REDUCTION_FACTOR,true,true);
    int32_t zCorrection = 0;
    #if defined(ENDSTOP_Z_BACK_ON_HOME)
    // If we want to go up a bit more for some reason
            if(ENDSTOP_Z_BACK_ON_HOME > 0)
    zCorrection -= axisStepsPerMM[Z_AXIS] * ENDSTOP_Z_BACK_ON_HOME * Z_HOME_DIR;
    currentPositionSteps[Z_AXIS] -= zBedOffset * axisStepsPerMM[Z_AXIS]; // Correct bed coating
            currentPositionSteps[Z_AXIS] = zMaxSteps - Printer::zBedOffset * axisStepsPerMM[Z_AXIS];
    #if NUM_EXTRUDER > 0
            currentPositionSteps[Z_AXIS] -= Extruder::current->zOffset;

    void Printer::homeAxis(bool xaxis,bool yaxis,bool zaxis)
    bool nocheck = isNoDestinationCheck();
    bool oldLaser = LaserDriver::laserOn;
    LaserDriver::laserOn = false;
        bool autoLevel = isAutolevelActive();

        if (!(X_MIN_PIN > -1 && Y_MAX_PIN > -1 && Z_MAX_PIN > -1
            Com::printErrorFLN(PSTR("Hardware setup inconsistent. Scara cannot home without proper endstops."));

    #if (xaxis = yaxis = zaxis = true)
       currentNonlinearPositionSteps[X_AXIS] = xMotorMinAngleSteps;
    currentNonlinearPositionSteps[Y_AXIS] = yMotorMaxAngleSteps;
    currentNonlinearPositionSteps[Z_AXIS] = zLength;
    if(zaxis) homeZAxis();
        if(yaxis) homeYAxis();
        if(xaxis) homeXAxis();

        Commands::printCurrentPosition(PSTR("homeAxis "));
    LaserDriver::laserOn = oldLaser;

  • I'm quite sure it will not work.

    In your transformation you are using int32_t so max. value is +/-2147483648 so squaring some steps > 46340 will overflow the value and you have garbage.

    Your homing neglects the fact that moveRelativeDistanceInSteps moves in cartesian not in scara coordinates. As I already said you need a special own function to move directly in scara coordinates. Don't implement home x/y as they are not possible. You can do homeZ since z is independent and homeScara if x and y is set, so pure G28 would home all.

    #if (xaxis = yaxis = zaxis = true)
    is a pain for programmer. #if is preprocessor compare so only works on const and it is always true if you omit # because you assign all axis true value and do not compare as you think. I guess you meant
    if(xaxis && yaxis && zaxis)

  • edited September 2016
    Hi again, 

    if I change int32_t to long or int64_t, will it solve the problem?
    btw, I don't know what is the different between long and int64_t.

    I'll take your advice to make new function to move directly to scara coordinate, 
    but I don't understand about home x/y are not possible.
    I think I have to really understand which one is home. 
    In the transformation I use center of print area as (0,0), my arm axis is outside the print area.
    which one is home? the center of print area of when all axis touch endstop ?

    I'm still considering about g28, where should the position of extruder be after g28... at the center of print area or at endstop?

    thanks for your advice and patient.

  • Hi repetier would you please check if this is right or not?

    void PrintLine::queueScaraMove(uint8_t check_endstops, uint8_t pathOptimize)
        float axisRotationRad[E_AXIS_ARRAY]; // Axis rotation in Rad
        p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
        if(Printer::isAllEMotors()) {
            p->flags |= FLAG_ALL_E_MOTORS;
        p->joinFlags = 0;
        if(!pathOptimize) p->setEndSpeedFixed(true);
        p->dir = 0;
        //Find direction
    Printer::zCorrectionStepsIncluded = 0;
        for(uint8_t axis = 0; axis < 4; axis++)
            p->delta[axis] = Printer::NonlinearDestinationSteps[axis] - Printer::currentNonlinearPositionSteps[axis];
            p->secondSpeed = Printer::fanSpeed;
            if(axis == E_AXIS)
                if(Printer::mode == PRINTER_MODE_FFF)
                    Printer::extrudeMultiplyError += (static_cast<float>(p->delta[E_AXIS]) * Printer::extrusionFactor);
                    p->delta[E_AXIS] = static_cast<int32_t>(Printer::extrudeMultiplyError);
                    Printer::extrudeMultiplyError -= p->delta[E_AXIS];
                    Printer::filamentPrinted += p->delta[E_AXIS] * Printer::invAxisStepsPerMM[axis];
                else if(Printer::mode == PRINTER_MODE_LASER)
                    p->secondSpeed = ((p->delta[X_AXIS] != 0 || p->delta[Y_AXIS] != 0) && (LaserDriver::laserOn || p->delta[E_AXIS] != 0) ? LaserDriver::intensity : 0);
                    p->delta[E_AXIS] = 0;
            if(p->delta[axis] >= 0)
                p->delta[axis] = -p->delta[axis];
            if(axis == X_AXIS || axis == Y_AXIS)
    axisRotationRad[axis] = p->delta[axis] / Printer::motorStepsPerRad[axis]
    axisRotationRad[axis] = p->delta[axis] * Printer::invAxisStepsPerMM[axis];
            if(p->delta[axis]) p->setMoveOfAxis(axis);
            Printer::currentNonlinearPositionSteps[axis] = Printer::NonlinearDestinationSteps[axis];
            if(newPath)   // need to delete dummy elements, otherwise commands can get locked.
            return; // No steps included
        float xydist2;
        if((p->isXYZMove()) && ((p->dir & XYZ_DIRPOS)^(Printer::backlashDir & XYZ_DIRPOS)) & (Printer::backlashDir >> 3))   // We need to compensate backlash, add a move
            uint8_t wpos2 = linesWritePos + 1;
            if(wpos2 >= PRINTLINE_CACHE_SIZE) wpos2 = 0;
            PrintLine *p2 = &lines[wpos2];
            memcpy(p2,p,sizeof(PrintLine)); // Move current data to p2
            uint8_t changed = (p->dir & XYZ_DIRPOS)^(Printer::backlashDir & XYZ_DIRPOS);
            float back_diff[4]; // Axis movement in mm
            back_diff[E_AXIS] = 0;
            back_diff[X_AXIS] = (changed & 1 ? (p->isXPositiveMove() ? Printer::backlashX : -Printer::backlashX) : 0);
            back_diff[Y_AXIS] = (changed & 2 ? (p->isYPositiveMove() ? Printer::backlashY : -Printer::backlashY) : 0);
            back_diff[Z_AXIS] = (changed & 4 ? (p->isZPositiveMove() ? Printer::backlashZ : -Printer::backlashZ) : 0);
            p->dir &= XYZ_DIRPOS; // x,y and z are already correct
            for(uint8_t i = 0; i < 4; i++)
                if( i = X_AXIS || i = Y_AXIS)
    float f = back_diff[i]*Printer::motorStepsPerRad[i];
    float f = back_diff[i]*Printer::axisStepsPerMM[i];
                p->delta[i] = abs((long)f);
                if(p->delta[i]) p->dir |= XSTEP << i;
            //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
            if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS]) p->primaryAxis = Y_AXIS;
            else if (p->delta[X_AXIS] > p->delta[Z_AXIS] ) p->primaryAxis = X_AXIS;
            else p->primaryAxis = Z_AXIS;
            p->stepsRemaining = p->delta[p->primaryAxis];
            //Feedrate calc based on XYZ travel distance
            xydist2 = back_diff[X_AXIS] * back_diff[X_AXIS] + back_diff[Y_AXIS] * back_diff[Y_AXIS];
                p->distance = sqrt(xydist2 + back_diff[Z_AXIS] * back_diff[Z_AXIS]);
                p->distance = sqrt(xydist2);
            // 56 seems to be xstep|ystep|e_posdir which just seems odd
            Printer::backlashDir = (Printer::backlashDir & 56) | (p2->dir & XYZ_DIRPOS);
            p = p2; // use saved instance for the real move

        //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
        if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS] && p->delta[Y_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Y_AXIS;
        else if (p->delta[X_AXIS] > p->delta[Z_AXIS] && p->delta[X_AXIS] > p->delta[E_AXIS]) p->primaryAxis = X_AXIS;
        else if (p->delta[Z_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Z_AXIS;
        else p->primaryAxis = E_AXIS;
        p->stepsRemaining = p->delta[p->primaryAxis];
    //        xydist2 = axisRotationRad[X_AXIS] * EEPROM::armLength() + axisRotationRad[Y_AXIS] * EEPROM::forearmLength();        
    xydist2 = axisRotationRad[X_AXIS] * axisRotationRad[X_AXIS] + axisRotationRad[Y_AXIS] * axisRotationRad[Y_AXIS];
                p->distance = RMath::max((float)sqrt(xydist2 + axisRotationRad[Z_AXIS] * axisRotationRad[Z_AXIS]),fabs(axisRotationRad[E_AXIS]));
                p->distance = RMath::max((float)sqrt(xydist2),fabs(axisRotationRad[E_AXIS]));
            p->distance = fabs(axisRotationRad[E_AXIS]);

  • #if DRIVE_SYSTEM == SCARA // Kusuma SCARA 
    void PrintLine::rotateXYRelativeAngleInSteps(int32_t x, int32_t y, float feedrate, bool waitEnd, bool checkEndstop,bool pathOptimize)
    #if MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) {
    x = 0;
    y = 0;
        float savedFeedrate = Printer::feedrate;
        int32_t destinationScaraSteps[X_AXIS] = Printer::currentNonlinearPositionSteps[X_AXIS] + x;
        int32_t destinationScaraSteps[Y_AXIS] = Printer::currentNonlinearPositionSteps[Y_AXIS] + y;
        Printer::feedrate = feedrate;
        if (!queueScaraMove(checkEndstop, pathOptimize, false))
            Com::printWarningFLN(PSTR("rotateXYRelativeAngleInSteps / queueScaraMove returns error"));
        Printer::feedrate = savedFeedrate;
        previousMillisCmd = HAL::timeInMilliseconds();
  • Sorry I'm quite sure my post above must be wrong.
    I rewrite it, would you please check if this is right or wrong?

    void PrintLine::rotateXYRelativeAngleInSteps(int32_t x, int32_t y, float feedrate, bool waitEnd, bool checkEndstop,bool pathOptimize)
    #if MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) {
    x = 0;
    y = 0;
        float savedFeedrate = Printer::feedrate;
        Printer::NonlinearDestinationSteps[X_AXIS] = Printer::currentNonlinearPositionSteps[X_AXIS] + x;
        Printer::NonlinearDestinationSteps[Y_AXIS] = Printer::currentNonlinearPositionSteps[Y_AXIS] + y;
    Printer::currentNonlinearPositionSteps[Z_AXIS] = Printer::currentPositionSteps[Z_AXIS];
        Printer::feedrate = feedrate;
    transformScaraStepsToCartesianSteps(Printer::currentNonlinearPositionSteps, Printer::currentPositionSteps);
        if (!queueScaraMove(checkEndstop, pathOptimize))
            Com::printWarningFLN(PSTR("rotateXYRelativeAngleInSteps / queueScaraMove returns error"));
        Printer::feedrate = savedFeedrate;
        previousMillisCmd = HAL::timeInMilliseconds();

    uint8_t PrintLine::queueScaraMove(uint8_t check_endstops, uint8_t pathOptimize)
        Printer::unsetAllSteppersDisabled(); // Motor is enabled now
    int32_t difference[2];
        PrintLine *p = getNextWriteLine();
    p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
    p->joinFlags = 0;
        if(!pathOptimize) p->setEndSpeedFixed(true);
        p->dir = 0;
        float axisRotationRad[2];
    for(fast8_t axis = 0; axis < 2; axis++)
            difference[axis] = Printer::NonlinearDestinationSteps[axis] - Printer::currentNonlinearPositionSteps[axis];
    if (difference[axis] >=0)
                difference[axis] = -difference[axis];
    if(difference[axis]) p->setMoveOfAxis(axis);
    axisRotationRad[axis] = fabs(difference[axis] / Printer::motorStepsPerRad[axis]);
            Printer::currentNonlinearPositionSteps[axis] = Printer::NonlinearDestinationSteps[axis];
    if (difference[X_AXIS] >= difference [Y_AXIS]) 
    p->primaryAxis = X_AXIS;
    p->stepsRemaining = difference[X_AXIS];
    p->primaryAxis = Y_AXIS;
    p->stepsRemaining = difference[Y_AXIS];
    p->distance = fabs(axisRotationRad[p->primaryAxis]);
    return true; // flag success

  • Hi, 
    when I upload, I get this warning message, is this okay?

    Sketch uses 143,716 bytes (56%) of program storage space. Maximum is 253,952 bytes.
    Global variables use 7,011 bytes (85%) of dynamic memory, leaving 1,181 bytes for local variables. Maximum is 8,192 bytes.
    Low memory available, stability problems may occur.

  • First more then 1kb free ram is ok. If you get below reduce subdivisions of move to get more free ram.

    Your homing understanding was incorrect I think. Think more abstract here. The reason for homing is to have a known starting position so X,y = 0,0 is at bed center. For Z it's just min/max endstop and easy. For xy position you need to know the angles of your scara, so cartesian koordinates do not help as you need to know the angles for this. So home XY means put scara angles at defined position. So you move here the angles towards your endstop in a way nothing gets damaged. So I assume first big arm, then second arm. Then you know where you are and can also set the cartesian coordinates.

    Your second version looks better but you need to base queueScaraMove on queueNonlinearMove more closely. Your version looks more like the cartesian version, but you are nonlinear and need to fill in the submoves of a move correctly. Here the critical part is that each move stays inside int16_t range for steps.

    Speaking of precision int32_t = long for 8/32 bit, int64_t is long long, but don't use it. Better convert it to float before squaring, then you have no range problems. Later you need it as float anyway for acos etc.

  • Hi repetier, thx for your answer.

    I set my x,y motor endstop when they folded. the position of x motor is outside bed and so is the extruder when touch endstop.
    when it folded, the arm nearly at 0 degree, pararel with x axis positive (let say 5 degree) and the forearm nearly 180 degree (let say 175 degree).
    I define xMotor Max Angle at 270 degree, xMotor Min Angle at 5 degree, xMotor Max Angle at 175 degree, 
    xMotor Max Angle at 0 degree. 
    so when they touch endstop x angle should be 5, y angle should be 175. this can be transformed to cartesian coordinate, I call this cartesian endstop coordinate, let say Xe, Ye.

    when it folded at endstop, there is a distance between shoulder and extruder. we can't print under that distance, so I define it as scara min radius. the max radius is arm + forearm length, we can't reach over that distance. 
    I define bed center and shoulder distance is average of min and max radius, so bed center is (0,0) and (Xe,Ye) is coordinate relative to bed origin.

    I did make the second version based on cartesian movement. because I think this is only used for homing and rotating the arm or forearm, not used for Gcode or linear move command... 
    My plan is move to endstop with scara move, translate the position to cartesian, then move to origin 0,0 with cartesian move... is it possible do this ?
    I agree that homing route is to prevent something hit by the arm, I think the most dangerous is when the go to endstop, go from endstop is not dangerous. But I will reconsider about move from endstop... 

    And because the function is not translating cartesian command to scara, I think sub segment is not nescessary... please correct me if this is wrong.

    btw, what is sub division?

  • >And because the function is not translating cartesian command to scara, I think sub segment is not >nescessary... please correct me if this is wrong.

    wrong! You always get the nonlinear bresenham algorithm for motion, so you must fill in the required data structures or nothing will happen or firmware will crash.

    >btw, what is sub division?

    Normally in linear systems we have 16 moves. For nonlinear this would be bad as we need to divide longer moves in many small moves. So normally we would need 100 moves in buffer, but that slows down path planning to much. So what we have is 1 move = buffer entry = 1 gcode if  possible. This move is split into x submoves with nonlinear move changes being linearized. If we can not have enough submoves as move is to long we also split it into more moves put into 1. level buffer. So every 1. level buffer move as x submoves from which we know they are on a line so we can optimize computations. These are stores as 16bit values to further reduce ram usage, so that is an other constraint. And this subdivision is what the nonlinear version does in addition which makes the code a bit longer.

    Also for nonlinear systems we introduced a virtual axis for bresenham algorthim as primary axis, because the dominant axis could switch from nonlinear behaviour. So we map the current dominant move for each segment to virtual axis.
  • Hi repetier, 

    now I understand why need submoves because of this.

    > Here the critical part is that each move stays inside int16_t range for steps.

    but I still don't understand the need of bresenham algorithm.
    I guess bresenham algorithm used to sync x, y, z, e movements?
    while I need the function to rotate one axis only at a time.
    for go to endstop, move z first, rotate elbow, then rotate shoulder.
    for go from endstop, rotate shoulder first, rotate elbow, then move z.
    the other movement I need is to calibrate the exact arm length & forearm length by measuring the linear distance with a caliper by rotating an axis some degree. this is also rotate one axis at a time, any other axis must not move at all.
    the last thing I need is to find endstop position in angle. to find this, I also need to rotate one axis only.

    if I use bresenham algorithm, isn't that mean that I have to translate an axis rotation movement to cartesian movement, divide them to sub segments, then translate again to axis rotation and I get a possibility of moving other axis that I don't want to?

    can I just make a function to just rotate one axis...
    maybe like rotateAnAxisInSteps(int32_t rotationSteps, fast8_t axis, float feedrate, bool waitEnd, bool checkEndstop, bool pathOptimize)
    divide to sub segments for  int16_t range consideration without bresenham algorithm?

  • edited October 2016
    like this ???

    void rotateAnAxisInSteps(int32_t rotationSteps, fast8_t axis, float feedrate, bool waitEnd, bool checkEndstop, bool pathOptimize)
    #if axis == X_AXIS && MOVE_X_WHEN_HOMED
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) {
    rotationSteps = 0;
    #elif axis == Y_AXIS && MOVE_Y_WHEN_HOMED
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) {
    rotationSteps = 0;
    float savedFeedrate = Printer::feedrate;
    Printer::feedrate = feedrate;
    for (fast8_t i = 0; i < VIRTUAL_AXIS_ARRAY; i++)
    Printer::NonlinearDestinationSteps[axis] = Printer::currentNonlinearPositionSteps[axis];
    fast8_t dir;
    if (rotationSteps >=0)
    dir = 1;
    dir =-1;    
    rotationSteps = -rotationSteps;
    uint16_t maxStepsPerSegment = 30000;
    if(rotationSteps > maxStepsPerSegment)
    uint8_t segments = floor(rotationSteps / maxStepsPerSegment);
    uint16_t remainingSteps = rotationSteps - maxStepsPerSegment * segments;
    for (fast8_t i = 0; i < segments; i++)
    Printer::NonlinearDestinationSteps[axis] = Printer::currentNonlinearPositionSteps[axis] + maxStepsPerSegment * dir;
    if (!queueRotationMove(axis, checkEndstop, pathOptimize))
    Com::printWarningFLN(PSTR("rotateAnAxisInSteps / queueRotationMove returns error"));
    uint16_t remainingSteps = rotationSteps;
    Printer::NonlinearDestinationSteps[axis] = Printer::currentNonlinearPositionSteps[axis] + remainingSteps * dir;
    if (!queueRotationMove(axis, checkEndstop, pathOptimize))
    Com::printWarningFLN(PSTR("rotateAnAxisInSteps / queueRotationMove returns error"));
    transformScaraStepsToCartesianSteps(Printer::currentNonlinearPositionSteps, Printer::currentPositionSteps);
        previousMillisCmd = HAL::timeInMilliseconds();
        Printer::feedrate = savedFeedrate;

    uint8_t PrintLine::queueRotationMove(fast8_t axis, uint8_t check_endstops, uint8_t pathOptimize) // Kusuma SCARA
        Printer::unsetAllSteppersDisabled(); // Motor is enabled now
    int32_t difference[VIRTUAL_AXIS_ARRAY];
    float axisRotationRad[VIRTUAL_AXIS_ARRAY];
    for (fast8_t i = 0; i < VIRTUAL_AXIS_ARRAY; i++)
    difference[i] = 0;
    axisRotationRad[i] = 0;
        PrintLine *p = getNextWriteLine();
    p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
    p->joinFlags = 0;
        if(!pathOptimize) p->setEndSpeedFixed(true);
        p->dir = 0;
    difference[axis] = Printer::NonlinearDestinationSteps[axis] - Printer::currentNonlinearPositionSteps[axis];
    if (difference[axis] >=0)
           difference[axis] = -difference[axis];
    if(difference[axis]) p->setMoveOfAxis(axis);
    axisRotationRad[axis] = fabs(difference[axis] / Printer::motorStepsPerRad[axis]);
        Printer::currentNonlinearPositionSteps[axis] = Printer::NonlinearDestinationSteps[axis];
    p->primaryAxis = axis;
    p->stepsRemaining = difference[axis];
    p->distance = fabs(axisRotationRad[p->primaryAxis]);
    return true; // flag success
    } // Kusuma SCARA

  • Code looks good on first look, but only testing will show.

    For your understanding, bresenham is always called for all moves as it is the only function moving the steppers. For bresenham x,y,z are the motors not coordinates. Normal queue delta has created them by calling your transformation to know how much steps per motor are used. You did the same now without the transformation since you want to move motors directly here. So that seems to be correct.
  • Hi again, tx for you guidance.
    it's successfully compiled, but I haven't tried the movements. I'll do it tomorrow.
    but now I have a strange result in ui & I don't know what have I done wrong with this one.

    #define UI_MENU_POSITIONS {UI_MENU_ADDCONDBACK &ui_menu_home_all,&ui_menu_home_x,&ui_menu_home_y,&ui_menu_home_z UI_SPEED_X UI_SPEED_Y UI_SPEED_Z UI_SPEED_X_ROTATION UI_SPEED_Y_ROTATION,&ui_menu_go_epos SERVOPOS_ENTRY,&ui_menu_go_offsets} 

    #define UI_SPEED_X_ROTATION ,&ui_menu_rotate_x_fast,&ui_menu_rotate_x_motor
    #define UI_SPEED_Y_ROTATION ,&ui_menu_rotate_y_fast,&ui_menu_rotate_y_motor
    #define UI_SPEED_X_ROTATION ,&ui_menu_rotate_x_motor
    #define UI_SPEED_Y_ROTATION ,&ui_menu_rotate_y_motor

    UI_MENU_ACTIONSELECTOR_T(ui_menu_rotate_x_fast, UI_TEXT_X_ANGLE_FAST_ID, ui_menu_x_angle_fast)
    UI_MENU_ACTIONSELECTOR_T(ui_menu_rotate_y_fast, UI_TEXT_Y_ANGLE_FAST_ID, ui_menu_y_angle_fast)
    UI_MENU_ACTIONSELECTOR_T(ui_menu_rotate_x_motor, UI_TEXT_X_ANGLE_POS_ID, ui_menu_x_angle)
    UI_MENU_ACTIONSELECTOR_T(ui_menu_rotate_y_motor, UI_TEXT_Y_ANGLE_POS_ID, ui_menu_y_angle)

    #define UI_TEXT_X_ANGLE_POS_ID 267 "Shoulder Position"
    #define UI_TEXT_Y_ANGLE_POS_ID 268 "Elbow Position"
    #define UI_TEXT_X_ANGLE_FAST_ID 269 "Shoulder Pos. Fast"
    #define UI_TEXT_Y_ANGLE_FAST_ID 270 "Elbow  Pos. Fast"



    #define UI_TEXT_X_ANGLE_POS_EN "Shoulder"
    #define UI_TEXT_Y_ANGLE_POS_EN "Elbow"
    #define UI_TEXT_X_ANGLE_FAST_EN "Shoulder. Fast"
    #define UI_TEXT_Y_ANGLE_FAST_EN "Elbow. Fast"

    the strange results are
    1. I get switched position between shoulder and elbow in LCD panel. 
    2. I get switched position in endstop status & angle status, angle status suppose to be at the top but it is at bellow.
    3. shoulder & angle endstop switched just in the rotating action, shoulder min endstop elbow max endstop suppose to be on, but the result shoulder max endstop on, and elbow min endstop on.
    4. in position menu it suppose to be then shoulder, but they are switched too.
    5. In position menu it suppose to be written Shoulder from this
    #define UI_TEXT_X_ANGLE_POS_EN "Shoulder"

    but it show Shoulder: 0.00 rad from one of this
    #define UI_TEXT_ACTION_XROTATION4A_EN "Shoulder:%x0 rad"
    #define UI_TEXT_ACTION_XROTATION2A_EN "Shoulder:%x0 rad"
    #define UI_TEXT_ACTION_XFAST_ROTATION4A_EN "Shoulder:%x0 rad"
    #define UI_TEXT_ACTION_XFAST_ROTATION2A_EN "Shoulder:%x0 rad"

    Pls help, tx

  • Hi, I've tried to plug the motors and endstops. And as I expected, there must be something wrong. 
    I think I've made mistake in the function, define, or somewhere.
    currentposition and currentnonlinearposition give the same number if I try to rotate any motor, 0 or -21474836.00

  • Have you also increased the translation number, meaning array length for translations?

    In menu always make sure that the counter for num entries stays correct. That gives strange effects if not.

    Also make sure defines are before they get used. Looks wrong order in your listing if it is not a copy error.
  • I think I did increase the translation number... If this is the one.

    uilang.h 101
    #define NUM_TRANSLATED_WORDS 295 

    before I add the last number in the list is 266, NUM_TRANSLATED_WORDS is 267. so the last number in the list after I add new words is 294, so I write NUM_TRANSLATED_WORDS is 295.

    I place defines of new words just a line after the original words... 

    I don't know what is this "the counter for num entries stays correct". It might be what I've done wrong.
    can you explain me more about this?

  • I get it, I skip one word in PGM_P const translations_en[NUM_TRANSLATED_WORDS] PROGMEM = {
    It's all in right position now in UI... tx

    So i guess my last problem is to set the movement properly.
    I still can't move my motors.

  • Hi repetier, can you pls explain to me what is the meaning of this function?

       if(p->delta[axis]) p->setMoveOfAxis(axis);

     I mean, I understand if(i=1), but I don't understand if(i)
    and I don't know what is p->setMoveOfAxis(axis); for... I see also d->setMoveOfAxis(axis); and can't figure out what are they.

Sign In or Register to comment.