User design speed curves for each joint on FPGA

[JamesNewton]

Currently we need to break up a move to straight into small movements and each of those has a trapezoidal speed profile.

If we wanted true control of the speed and position while moving in a straight line, we will need to be able to pass in complex speed curves at a very high resolution. This could be implemented as a series of trapezoidal movements with different end speeds or as a lookup table of speeds that is interpolated during the move by the FPGA.

[JamesWigglesworth]

The first stab of this has been done in this commit of DexRun.c.

It turns out the functionally has been there for a while in the form of DMA playback. Originally this was used to do recordings and playback before DDE. We were able to covert complex positional curves into this format which allows us direct control of the stepper motors in time.

The first functionality added is an improved move_to_straight. This uses the 'Cartesian...' set parameters.

Here is the DDE code to run it (only works in DDE versions 2.5.17 and not 3.6.3 but possibly later versions):

new Job({
    name: "Straight_Test",
    do_list: [
        Dexter.set_parameter("MaxSpeed", 25),
        Dexter.set_parameter("StartSpeed", 0),
        Dexter.move_to([-0.1, 0.5, 0.3], [0, 0, -1], [1, 1, 1]),
        make_ins("F"),

        Dexter.set_parameter("CartesianSpeed", Math.round(0.4/_um)),
        Dexter.set_parameter("CartesianAcceleration", Math.round(1/_um)),
        Dexter.set_parameter("AngularSpeed", Math.round(100/_arcsec)),
        make_ins("T", 0.1, 0.5, 0.3, 0, 0, -1, 1, 1, 1),

        Dexter.set_parameter("MaxSpeed", 25),
        Dexter.set_parameter("StartSpeed", 0),
        Dexter.move_all_joints([0, 0, 0, 0, 0]),
        make_ins("F")
    ]
})

What still needs to be done: -It takes like a second or two between movements (which is a lot better than 5 minutes)     -I couldn't figure out how to write directly to the DMA or use the CalTables array and load the table.      What was able to to do was write everything out to a file then load that file back in and do the normal 'o' oplet.     -I'm not being efficient with the file size, there are a bunch of zero step data points getting put in there.      As the file size goes down the computation time will go up so we'll have figure the right balance of this trade off.

-An input into this function is 'max_speed_guess' which is used to calculate the minimum time step that could occur.  This minimum time step is what I take time samples at (like a frame rate).  If a joint is moving at exactly the speed of max_speed_guess it will move at 1 motor step per time step.  If it calculates more than 1 motor step per this time step all of the math breaks.  Effectively I want this to error and not move if this occurs, but I don't really have a mechanism for this.  Right now it will actually move until it reaches this max_speed_guess then come to an abrupt and violent stop.  (max_speed_guess can be set with a the set param of 'AngularSpeed' for now.)

-I don't have a mechanism for letting the user know an xyz was out of bounds (or if a joint was passed boundary either).

-It only works with a direction of [0, 0, -1] for both end points.  Eventually I'd like to allow different start and end directions and interpolate between them.  There is an issue for calculating the acceleration curve when the start and end directions are different but the start and end xyz's are the same.

-CartesianStartSpeed and CartesianEndSpeed can only be zero.

-It is not possible to queue multiple 'T' oplets, it only returns the robot status after the robot status is complete.  This is actually kind of nice because it makes it synchronous by default, instead using the 'F' oplet (empty_instruction_queue)

MoveRobotStraight(), which is what is called by the 'T' oplet, has been replace with a new algorithm. This algorithm generates the raw step, direction, and duration format that directly controls the motors through the FPGA via the DMA. This allows maximum smoothness and speed of straight line motion.

New functions: unsigned int make_bin_string(bool step[], bool dir[], unsigned int time) void position_curve_to_dma(void time_to_J_angles(float)) position_curve_straight(float t)

position_curve_straight is just the first function to be used for this direct motor control. It takes an argument of 't' for time in seconds and results in a global variable time_to_J_angles_result being set to a set of joint angles in arcseconds.

A number of different functions can be written in this format including: -position_curve_playback_recording -position_curve_circle -position_curve_polyline -position_curve_natural_cubic_spline -position_curve_constant_acceleration etc.

[JamesNewton]

I'll try to help with writing directly to RAM.

"I'm not being efficient with the file size, there are a bunch of zero step data points getting put in there. As the file size goes down the computation time will go up so we'll have to figure the right balance of this trade off." 

I don't see why the computation should be difficult... just compute based on the minimum time slice as before, and keep the current value rather than immediately writing it out, then compute the next value. If there is no step change, add the two times, keep that (don't write it out) and compute another slice. If there IS a change in the steps, then write out the prior, saved, value, and hold on to the new value as before. That can't take more than a few cycles... On the other hand, with 2 million points, I'm not sure how critical it is to save space, once we get past writing it to a file. Oh... maybe doing all those minimum-time-slice computations which result in no changes is what you were talking about?

On max_speed_guess, is the problem that the guess is sometimes wrong? And you don't know it's wrong until the time slices are overwhelmed and then it's too late. That could be caught now and just don't load the file, but as we move to doing 

Maybe you can vary the width of the time slice and increase or decrease it to make sure that there are enough slices between a step to ensure accurate timing.

Or use the floating point values in the step time computation to put out an exact delay instead of slicing up time into fixed units. 

"...letting the user know an xyz was out of bounds (or if a joint was passed boundary either)." XYZ is just the donut thing, right? And joints out of bounds can also be tracked. Just more work, right? Maybe I can help there.

Queueing (in firmware, no FPGA) and different end and start speeds will all come once we can go direct to RAM. 

[cfry]

"I don't see why the computation should be difficult... just compute based on the minimum time slice as before, and keep the current value rather than immediately writing it out, then compute the next value. If there is no step change, add the two times, keep that (don't write it out) and compute another slice. If there IS a change in the steps, then write out the prior, saved, value, and hold on to the new value as before."

Sounds like a good algorithm to me. Now once we have that, consider generalizing it a bit, from "If there is no step change" to "If the change is below X" where we can decide how high to set this "low pass filter". ie maybe sometimes we're willing to trade off "perfect fidelity" with "something not so perfect but a heck of a lot less data." Ie "not so lossy compression". I'm not sure my above algorithm would do it, but taking out the little jitters I'd think would be better than the original, at least for some purposes.

On Mon, Sep 14, 2020 at 4:52 PM JamesNewton notifications@github.com wrote:

I'll try to help with writing directly to RAM.

"I'm not being efficient with the file size, there are a bunch of zero step data points getting put in there. As the file size goes down the computation time will go up so we'll have to figure the right balance of this trade off."

I don't see why the computation should be difficult... just compute based on the minimum time slice as before, and keep the current value rather than immediately writing it out, then compute the next value. If there is no step change, add the two times, keep that (don't write it out) and compute another slice. If there IS a change in the steps, then write out the prior, saved, value, and hold on to the new value as before. That can't take more than a few cycles... On the other hand, with 2 million points, I'm not sure how critical it is to save space, once we get past writing it to a file. Oh... maybe doing all those minimum-time-slice computations which result in no changes is what you were talking about?

On max_speed_guess, is the problem that the guess is sometimes wrong? And you don't know it's wrong until the time slices are overwhelmed and then it's too late. That could be caught now and just don't load the file, but as we move to doing

Maybe you can vary the width of the time slice and increase or decrease it to make sure that there are enough slices between a step to ensure accurate timing.

Or use the floating point values in the step time computation to put out an exact delay instead of slicing up time into fixed units.

"...letting the user know an xyz was out of bounds (or if a joint was passed boundary either)." XYZ is just the donut thing, right? And joints out of bounds can also be tracked. Just more work, right? Maybe I can help there.

Queueing (in firmware, no FPGA) and different end and start speeds will all come once we can go direct to RAM.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/HaddingtonDynamics/Dexter/issues/96#issuecomment-692307348, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAJBG7KPGNI6Z43RQ72JQBTSFZ67DANCNFSM4OY7E3WQ .