Reported positions are incorrect after adding custom SCARA kinematics

[JelmerV]

Hi, I plan to use grblHAL for the controls of a SCARA robot and am currently implementing the custom kinematics for that. I seem to have the kinematics and the line segmentation function working now. This can be found on my fork at https://github.com/JelmerV/core

However, I also added the joint angles to the real-time reports and those seem incorrect. The second angle is always the negative of the first. I printed the line segment output and those are correct, but grbl does not move to those requested angles correctly. I wrote a Python script that sends some test code and visualizes the robot configuration. This is quite helpful in confirming what the robot is doing. This script can be found at: https://github.com/JelmerV/grblhal_development_utils/blob/master/tests/scara_grbl_visualizer.py

The printed output from the segmentation function looks as follows, here the requested joint angles are correct.

>> G1 X300 Y-100
seg_line: itrs=72, do_segments=1, distance=140.470963, delta=-1.397546,-1.400423,0.000000
seg_line: itrs=71, do_segments=1, segment_target=397.828217,-1.970370 steps=-1.031736,1.506590
seg_line: itrs=70, do_segments=1, segment_target=396.430664,-3.370793 steps=-1.035490,1.505154
seg_line: itrs=69, do_segments=1, segment_target=395.033112,-4.771216 steps=-1.039261,1.503691
seg_line: itrs=68, do_segments=1, segment_target=393.635559,-6.171639 steps=-1.043049,1.502203`

Then the robot starts moving and the real-time reports look like this: (Qj are the current joint angles)

<Run|MPos:399.226,-0.570,0.000|Bf:0,1023|FS:50,10|Pn:P|Qj:-1.508,1.508>
<Run|MPos:382.018,-18.101,0.000|Bf:0,1023|FS:50,10|Pn:P|Qj:-1.488,1.488>
<Run|MPos:358.496,-41.157,0.000|Bf:0,1023|FS:50,10|Pn:P|Qj:-1.456,1.456>

What might be going wrong here? I feel like I am missing something obvious, but it is also not really clear to me what the returned values from the different functions should be.

I would really appreciate it if you could take a look and maybe steer me in the right direction. Thanks!

[terjeio]

First thing to check is that the forward then backward transform produces the same output as the input. Have you verified that?

The output from the forward transform should result in the motors moving so that the tooltip follows the trajectory of the programmed move.

FYI I am in the middle of adding support for some ethernet modules to grblHAL, until I have completed this I am not able to look into your code in any detail.

[JelmerV]

I have tested some more and all conversions seem to work correctly and I think grbl is also moving correctly. I just do not yet have a physical test setup. But the reported MPos is correct.

However, I also added an additional part to the real-time reports for the joint angles. I implemented that as follows:

static void report_angles (stream_write_ptr stream_write, report_tracking_flags_t report)
{
    if (true) {
        char *q1 = ftoa(sys.position[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm, 3);
        char *q2 = ftoa(sys.position[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm, 3);
        stream_write("|Qj:");
        stream_write(q1);
        stream_write(",");
        stream_write(q2);
    }
    if (on_realtime_report){
        on_realtime_report(stream_write, report);
    }
}

And I enable this within the init as:

    // add q angles to realtime report
    on_realtime_report = grbl.on_realtime_report;
    grbl.on_realtime_report = report_angles; 

Now it seems that the way I get the current motor steps is not correct. The value obtained from sys.position[A_MOTOR] is incorrect. Note that A_MOTOR is equal to X_AXIS.

Which object or function can I use to obtain the current motor steps from within the on_realtime_report function?

[terjeio]

ftoa() writes to a static buffer, the second call will overwrite it. Change your code like this:

        stream_write("|Qj:");
        stream_write(ftoa(sys.position[A_MOTOR] / settings.axis[A_MOTOR].steps_per_mm, 3));
        stream_write(",");
        stream_write(ftoa(sys.position[B_MOTOR] / settings.axis[B_MOTOR].steps_per_mm, 3));

[JelmerV]

Amazing, that fixes that!

I think the kinematics are working correctly then. I will do some more tests, and still need to add the homing/limits related functions.

Thanks!