3D Printed Robot Arm - Acceleration / Deceleration Planner Algorithm?

Hi All,

I’m working on a 3D Printed Robot Arm using the Dagor ESP32 board running SimpleFOC. As you can see in the video I’ve got it far enough along that I’m testing motion on each of the 6 axes.

My approach for motion on the robot arm is to let the user manually move the arm into the correct position, record it, then move to the next position, record that, etc. Then the user would play back the recorded positions sort of like playing Gcode being sent to a 3D printer. When playing the recorded positions back, I would let them specify any velocity (slower/faster) and I would calculate the acceleration and deceleration for each move.

Does anybody have any algorithm that already does this?

It would seem that others have likely run into this problem. In the world of stepper motors there is a popular library called AccelStepper. There’s also the open source Grbl and TinyG code for steppers that do motion planning. Grbl uses trapezoidal planning to keep it simple. TinyG uses S-curve planning for even smoother acceleration/deceleration. Both of those also do motion planning whereby they don’t slow all the way down to zero as they head into the next move if the angle of the move is not drastic.

At a minimum, I’d love to just do a simple trapezoidal acceleration/deceleration algorithm inside the SimpleFOC loop like the below graph:
image

Or it would be nice to get a bit fancier and do more of the S-curve:
image

This is an example of how TinyG visualizes its jerk acceleration / deceleration. I would LOVE to implement an algorithm like this.

3 Likes

Hi @jlauer very interesting project :slight_smile:

I think what you are looking for is something like Trapezoidal Path Planner which, as far as I know, is not currently implemented at SimpleFOC level … but in my opinion it would be a very nice feature to have :slight_smile:

My knowledge in this field is extremely limited, but I think you can externally generate this kind of trajectory and command it to the SimpleFOC device.

Regards

PS: Just curious … do you use some kind of gear at your arm like planetary, harmonic , cycloidal… or are in direct drive?

Hi @JorgeMaker, that thread looks very promising. It appears the code added to ODrive was based on a research paper (https://hal.archives-ouvertes.fr/hal-01575057/document) which has a key chart in it:

There is a Python file they added to ODrive which seems to be the first test implementation for the algorithm. This could be super helpful. (https://github.com/odriverobotics/ODrive/blob/a8461c40fd6d97991adc9d553db3fdbce4a01dc4/tools/motion_planning/PlanTrap.py)

On your question, yes, the robot arm uses a 20:1 planetary gearbox that is backdrivable so each axis can be placed into a light torque mode so the user can position the robot arm where they want to record the position. I’m getting about 12NM of torque out of each actuator, which as you can see in the video is enough power for the axes at the base to lift the entire arm, which is quite heavy. It was a risk whether this would work, but I’m quite pleased with how things are coming together.

1 Like

Very interesting … I am developing a similar project, but I decided to design my own custom PCBs to run SimpleFOC and it is taking me longer than I initially thought.

For the issue of arm control, I had thought of trying both your strategy of recording positions and then playing it and also willing to try some kind of integration with ROS … an environment that from what little I know can offer solutions to the planning of trajectories at a higher level than the one with the actuators

Want to collaborate? This robot arm is super fun, but it is a lot of work. I’m doing it because I think the world needs an inexpensive 6 DOF robot arm that actually functions. Inexpensive BLDCs, drivers, and projects like SimpleFOC are breakthrough territory for the maker community, which then makes this possible.

Here’s an image of the actuator and how complicated it is. It’s quite the 3D printing process to pull each one off, but once you get the hang of it, they come together nicely. The cost is about $100 per actuator. So, you’re talking an entire robot arm for maybe $700.

2 Likes

Very nice and compact design :slight_smile: Yes we can share experiences, knowledge, pains and joys :slight_smile:

My initial plan was to develop a complete actuator unit [PCB, scketch, motor selection and “gearbox” design] and then clone it in sufficient quantity to develop a robotic arm … but due to a shortage of electronic components by afraid of running out of supplies or having to pay absurd prices, I had to modify my initial design to cut costs and advance PCB production before the full actuator design was completed.

I have a couple of questions for you … :slight_smile:

  • Did you 3D print your planetary gearbox or incorporated something like this ? If 3D printed … Were you able to get an acceptable backlash?

  • Do you have any recommendations or references for the BLDC motor used? I have many doubts about which engines to choose since I do not want to fall short or spend too much on excessively powerful engines for this type of application. My heart says buy a little beast, something like this but my wallet says look for something cheaper :slight_smile:

@JorgeMaker

Would that motor run on 12V? the specs say 5V. I don’t have any gimbal motor experience.

Not sure … the spec says you can get "Torque: 4970g @ 5V & 0.75A " but do not specify any upper limit as far as I know.

I guess there has to be an upper limit in terms of current and voltage or a any combination of both, but I couldn’t find it. This brand offers a very poor/undefinded specifications :frowning:

The testing setup for my board has the BGM4108-130. For this motor, a little brother of BGM8108-90, the specs say “Torque: 700g @ 5V & 0.43A” … I have tested at 11V ,which is the limit of my regulated power supply, without any problems and with good results. I plan to test it to 24V once I got more clones of my board, … for the moment I do not want to risk :slight_smile:

This is the motor I’m using Gartt ML5010 @ $33/each in bulk. https://www.amazon.com/gp/product/B01ALD07RG/ref=ox_sc_saved_title_1?smid=A2VS8HDVIIQ0HN&psc=1

All gears are 3D printed. There’s a category in @David_Gonzalez 's Discord for this robot arm. https://discord.com/channels/805671632843964426/848349613207060562

2 Likes

You have 0.24mm^2 enamelled copper wire, if I see it correctly - so there will be a max current rating for this. And the insulation will have a max voltage, but this will be much higher than 12V, I think.

So 12V will be 1.25A or thereabouts - the motor will get hot after a while, but it should be fine to run at 12V.

I’d like to keep this thread focused on the acceleration / deceleration algorithm as I really could use the help of this community on that.

1 Like

On the original topic, I found a few links in my list about motion control for arms:





2 Likes

Many many years without using Matlab but it seems that trajectory planning can be used.

If you get a file with calculated target positions, you can use a Python script to reproduce those calculated paths following your desired profiles. … It would be great if such a thing was done in SimpleFOC, but at the moment, as far as I know, it can’t be done by the library.

Maybe, I’m not sure something similar can be implemented at sketch level by intercepting position commands and then interpolating the target position to achieve with intermediate target positions that meet your desired profile in terms in terms of velocity and accelerations … but not sure.

I am nut sure it will apply to your specific need, but in the OpenPnp Community there has been some development on S-curve motion control driven mainly by Mark. Take a look in the source.

1 Like

Making progress here with that post from @JorgeMaker regarding the Trapezoidal Path Planner. This algorithm does seem to be able to generate the position the SimpleFOC device should be at for any arbitrary time and calculates that position based on an acceleration / deceleration profile.

I’ve heavily tweaked the Python code (https://github.com/odriverobotics/ODrive/blob/a8461c40fd6d97991adc9d553db3fdbce4a01dc4/tools/motion_planning/PlanTrap.py) and this is exactly what I need for giving SimpleFOC the positions each time through the loop (or each N times through the loop).

As an example, here’s a typical move I’d make on the robot arm. I’d go from position 0 (radians) to position 30 (radians, roughly 5 turns of motor). That is shown on the Y axis.

I would make this move very slowly if I’m moving a heavy object. So I would use velocity of 10 radians/second and an acceleration of 5 radians/second/second.

The Python code gives me a perfect acceleration / coast / deceleration profile. It calculates that my total move time is 5 secs. 2 seconds spent accelerating, 1 second in coast, and 2 seconds decelerating.

Start position (rads): 0
End position (rads): 30
Velocity for move (rads/s): 10
Acceleration (rads/s/s): 5

image

Here is a test of the code that would get called each time through the loop at any arbitrary time to find what the current position should be, so that we can update the position target in SimpleFOC as fast as possible.

t (seconds): 1.00
Acceleration stage
y (pos): 2.50
y_Accel (): 10.00
yd (vel): 5.00
ydd (acc): 5.00

t (seconds): 2.50
Coasting stage
y (pos): 15.00
y_Accel (): 10.00
yd (vel): 10.00
ydd (acc): 0.00

t (seconds): 4.00
Deceleration stage
y (pos): 27.50
y_Accel (): 10.00
yd (vel): 5.00
ydd (acc): -5.00

Here is the code that would get run each time through the loop:

Python Code calc_step_pos_at_arbitrary_time()
def calc_step_pos_at_arbitrary_time(t, Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf):

    # t is the current time
    # t would be calculated based on microseconds() passed
    # since the start of the current move
    #t = 4.5

    # We only know acceleration (Ar and Dr), so we integrate to create
    # the velocity and position curves
    # we should do this once at the start of the move, not
    # each time thru this step method
    y_Accel = Xi + Vi*Ta + 0.5*Ar*Ta**2

    y = None
    yd = None
    ydd = None

    print()
    print("t (seconds): {:.2f}".format(t))

    if t < 0: # Initial conditions
        print("Initial conditions")
        y   = Xi
        yd  = Vi
        ydd = 0
    elif t < Ta: # Acceleration
        print("Acceleration stage")
        y   = Xi + Vi*t + 0.5*Ar*t**2
        yd  = Vi + Ar*t
        ydd = Ar
    elif t < Ta+Tv: # Coasting
        print("Coasting stage")
        y   = y_Accel + Vr*(t-Ta)
        yd  = Vr
        ydd = 0
    elif t < Tf: # Deceleration
        print("Deceleration stage")
        td     = t-Tf
        y   = Xf + 0*td + 0.5*Dr*td**2
        yd  = 0 + Dr*td
        ydd = Dr
    elif t >= Tf: # Final condition
        print("Final condition")
        y   = Xf
        yd  = 0
        ydd = 0
    else:
        raise ValueError("t = {} is outside of considered range".format(t))

    print("y (pos): {:.2f}".format(y))
    print("y_Accel (): {:.2f}".format(y_Accel))
    print("yd (vel): {:.2f}".format(yd))
    print("ydd (acc): {:.2f}".format(ydd))

    return (y, yd, ydd)

Then, just to test the algorithm further, let’s decrease the acceleration to 2 radians/sec/sec, which is slow enough where we may never get to a coast phase because we reach our final destination before the maximum velocity of 10 radians/sec can be achieved.

Start position (rads): 0
End position (rads): 30
Velocity for move (rads/s): 10
Acceleration (rads/s/s): 2

image

This time it takes us 7.74 seconds to reach our destination. 3.87 on accelerate and 3.87 on decelerate. We never reach coast, so we have a triangle shape move rather than a trapezoidal shape.

Thus this algorithm is going to do the job for us. Here’s the main calculation code that would get run once at the start of the move:

Python Code calc_plan_trapezoidal_path_at_start()
def calc_move_parameters_at_start(Xf, Xi, Vi, Vmax, Amax, Dmax):
    dX = Xf - Xi    # Distance to travel
    stop_dist = Vi**2 / (2*Dmax)  # Minimum stopping distance
    dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement
    s = np.sign(dX - dXstop)   # Sign of coast velocity (if any)
    Ar = s*Amax     # Maximum Acceleration (signed)
    Dr = -s*Dmax    # Maximum Deceleration (signed)
    Vr = s*Vmax     # Maximum Velocity (signed)

    # If we start with a speed faster than cruising, then we need to decel instead of accel
    # aka "double deceleration move" in the paper
    if s*Vi > s*Vr:
        print("Handbrake!")
        Ar = -s*Amax

    # Time to accel/decel to/from Vr (cruise speed)
    Ta = (Vr-Vi)/Ar
    Td = -Vr/Dr

    # Integral of velocity ramps over the full accel and decel times to get
    # minimum displacement required to reach cruising speed
    dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0

    # Are we displacing enough to reach cruising speed?
    if s*dX < s*dXmin:
        print("Short Move:")
        # From paper:
        # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar))
        # Simplified for less divisions:
        Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar))
        Ta = max(0, (Vr - Vi)/Ar)
        Td = max(0, -Vr/Dr)
        Tv = 0
    else:
        print("Long move:")
        Tv = (dX - dXmin)/Vr # Coasting time

    Tf = Ta+Tv+Td

    print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi))
    print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax))
    print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin))
    print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr))
    print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td))

    return (Ar, Vr, Dr, Ta, Tv, Td, Tf)

Here is the final code that now needs to be transposed to C code for running with SimpleFOC. I think I’ll add a commander command like G for Gcode and have users submit something like “G30 V10 A5” for move 30 radians at velocity of 10 (rads/s) at acceleration of 5 (rads/s/s).

# Copyright (c) 2021 John Lauer for modifications
# 
# Original copyrights and code by
# Original URL https://github.com/odriverobotics/ODrive/blob/a8461c40fd6d97991adc9d553db3fdbce4a01dc4/tools/motion_planning/PlanTrap.py
# Copyright (c) 2018 Paul Guénette
# Copyright (c) 2018 Oskar Weigl

# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:

# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.

# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# This algorithm is based on:
# FIR filter-based online jerk-constrained trajectory generation
# https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf

def calc_plan_trapezoidal_path_at_start(Xf, Xi, Vi, Vmax, Amax, Dmax):
    dX = Xf - Xi    # Distance to travel
    stop_dist = Vi**2 / (2*Dmax)  # Minimum stopping distance
    dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement
    s = np.sign(dX - dXstop)   # Sign of coast velocity (if any)
    Ar = s*Amax     # Maximum Acceleration (signed)
    Dr = -s*Dmax    # Maximum Deceleration (signed)
    Vr = s*Vmax     # Maximum Velocity (signed)

    # If we start with a speed faster than cruising, then we need to decel instead of accel
    # aka "double deceleration move" in the paper
    if s*Vi > s*Vr:
        print("Handbrake!")
        Ar = -s*Amax

    # Time to accel/decel to/from Vr (cruise speed)
    Ta = (Vr-Vi)/Ar
    Td = -Vr/Dr

    # Integral of velocity ramps over the full accel and decel times to get
    # minimum displacement required to reach cruising speed
    dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0

    # Are we displacing enough to reach cruising speed?
    if s*dX < s*dXmin:
        print("Short Move:")
        # From paper:
        # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar))
        # Simplified for less divisions:
        Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar))
        Ta = max(0, (Vr - Vi)/Ar)
        Td = max(0, -Vr/Dr)
        Tv = 0
    else:
        print("Long move:")
        Tv = (dX - dXmin)/Vr # Coasting time

    Tf = Ta+Tv+Td

    # We only know acceleration (Ar and Dr), so we integrate to create
    # the velocity and position curves
    # we should do this once at the start of the move, not
    # each time thru this step method
    y_Accel = Xi + Vi*Ta + 0.5*Ar*Ta**2

    print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi))
    print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax))
    print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin))
    print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr))
    print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td))
    print("y_Accel: {:.2f}".format(y_Accel))
    print("Tf (full move time): {:.2f}".format(Tf))
    

    return (Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel)

def calc_step_pos_at_arbitrary_time(t, Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel):

    # t is the current time
    # t would be calculated based on microseconds() passed
    # since the start of the current move
    #t = 4.5

    y = None
    yd = None
    ydd = None

    print()
    print("t (seconds): {:.2f}".format(t))

    if t < 0: # Initial conditions
        print("Initial conditions")
        y   = Xi
        yd  = Vi
        ydd = 0
    elif t < Ta: # Acceleration
        print("Acceleration stage")
        y   = Xi + Vi*t + 0.5*Ar*t**2
        yd  = Vi + Ar*t
        ydd = Ar
    elif t < Ta+Tv: # Coasting
        print("Coasting stage")
        y   = y_Accel + Vr*(t-Ta)
        yd  = Vr
        ydd = 0
    elif t < Tf: # Deceleration
        print("Deceleration stage")
        td     = t-Tf
        y   = Xf + 0*td + 0.5*Dr*td**2
        yd  = 0 + Dr*td
        ydd = Dr
    elif t >= Tf: # Final condition
        print("Final condition")
        y   = Xf
        yd  = 0
        ydd = 0
    else:
        raise ValueError("t = {} is outside of considered range".format(t))

    print("y (pos): {:.2f}".format(y))
    print("y_Accel (): {:.2f}".format(y_Accel))
    print("yd (vel): {:.2f}".format(yd))
    print("ydd (acc): {:.2f}".format(ydd))

    return (y, yd, ydd)

if __name__ == '__main__':

    Xf = 30 # Position we're moving to (rads)
    Xi = 0 # Initial position (rads)
    Vi = 0 # Initial velocity (rads/s)
    Vmax = 10 # Velocity max (rads/s)
    Amax = 5 # Acceleration max (rads/s/s)
    Dmax = Amax # Decelerations max (rads/s/s)

    # Plan the path
    (Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel) = calc_plan_trapezoidal_path_at_start(Xf, Xi, Vi, Vmax, Amax, Dmax)
    
    # Example calls each time through loop during move
    # Time = 1 second
    (Y, Yd, Ydd) = calc_step_pos_at_arbitrary_time(1, Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel)
    # Time = 2.5 seconds
    (Y, Yd, Ydd) = calc_step_pos_at_arbitrary_time(2.5, Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel)
    # Time = 4 seconds
    (Y, Yd, Ydd) = calc_step_pos_at_arbitrary_time(4, Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf, y_Accel)
1 Like

Got it working! Thanks for all the help from this thread. I’m amazed how quickly it came together. You can see in the video how well it works.

Examples:
G30 to move to position 30 (rads)
GV100 to change velocity to 100 (rads/s)
GA5 to change acceleration to 5 (rads/s/s)

Here are the 2 files I added to the Dagor controller’s code base to get the G command added into the SimpleFOC commander. Keep in mind this is very messy code. Lots of globals. Not well contained into a struct. If you issue a new Gcode move in the middle of an existing one the behavior is weird. However, this is an amazing start. Would love help from the community to refine the code.

image

This is the main file that’s as close to a port of the Python script I posted above as I could get it on quick order.

ACCELDECEL_PLANNER.ino
// Acceleration / Deceleration Planner for SimpleFOC & Dagor Controller
// Copyright (c) 2021 John Lauer for ACCELDECEL_PLANNER.ino
// This library allows you to control SimpleFOC with a plan for moves at a certain velocity as well
// as acceleration and deceleration at a certain rate. This code follows the same license as the original code
// given credit to below.
// 
// Credit for most of this concept and much of the code used as a starting point for this library goes to 
// the code inside ODrive available at:
// https://github.com/odriverobotics/ODrive/blob/a8461c40fd6d97991adc9d553db3fdbce4a01dc4/tools/motion_planning/PlanTrap.py
// 
// # Original copyrights and code by
// # Copyright (c) 2018 Paul Guénette
// # Copyright (c) 2018 Oskar Weigl

// # Permission is hereby granted, free of charge, to any person obtaining a copy
// # of this software and associated documentation files (the "Software"), to deal
// # in the Software without restriction, including without limitation the rights
// # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// # copies of the Software, and to permit persons to whom the Software is
// # furnished to do so, subject to the following conditions:

// # The above copyright notice and this permission notice shall be included in all
// # copies or substantial portions of the Software.

// # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// # SOFTWARE.

// # This algorithm is based on:
// # FIR filter-based online jerk-constrained trajectory generation
// # https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf

// Symbol                     Description
// Ta, Tv and Td              Duration of the stages of the AL profile
// Xi and Vi                  Adapted initial conditions for the AL profile
// Xf                         Position set-point
// s                          Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax  Kinematic bounds
// Ar, Dr and Vr              Reached values of acceleration and velocity

// float Xf = 30; // # Position we're moving to (rads)
// float Xi = 0; // # Initial position (rads)
// float Vi = 0; // # Initial velocity (rads/s)
float Vmax_ = 1000; //10; // # Velocity max (rads/s)
float Amax_ = 20; //5; // # Acceleration max (rads/s/s)
float Dmax_ = Amax_; // # Decelerations max (rads/s/s)

//struct Step_t {
  float Y_;
  float Yd_;
  float Ydd_;
//};

// Private variables
float Tf_, Xi_, Xf_, Vi_, Ar_, Dr_, Vr_, Ta_, Td_, Tv_, yAccel_;

float sign(float val) {
    if (val < 0) return -1.0f;
    if (val == 0) return 0.0f;
    //if val > 0:
      return 1.0f;
}

// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {
    if (val < 0) return -1.0f;
    return 1.0f;
}

// This method should get called at the start of a move to generate all of the global variables
// for the duration of the move. These numbers will be used each time you call calc_step_pos_at_arbitrary_time()
// during the loop() because of course you would not want to calc these globals each time.
bool calc_plan_trapezoidal_path_at_start(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) {

    float dX = Xf - Xi;  // Distance to travel
    float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
    float dXstop = sign(Vi) * stop_dist; // Minimum stopping displacement
    float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
    Ar_ = s * Amax;  // Maximum Acceleration (signed)
    Dr_ = -s * Dmax; // Maximum Deceleration (signed)
    Vr_ = s * Vmax;  // Maximum Velocity (signed)

    // If we start with a speed faster than cruising, then we need to decel instead of accel
    // aka "double deceleration move" in the paper
    if ((s * Vi) > (s * Vr_)) {
        Ar_ = -s * Amax;
    }

    // Time to accel/decel to/from Vr (cruise speed)
    Ta_ = (Vr_ - Vi) / Ar_;
    Td_ = -Vr_ / Dr_;

    // Integral of velocity ramps over the full accel and decel times to get
    // minimum displacement required to reach cuising speed
    float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;

    // Are we displacing enough to reach cruising speed?
    if (s*dX < s*dXmin) {
        // Short move (triangle profile)
        Vr_ = s * sqrt((Dr_*sq(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
        Ta_ = max(0.0f, (Vr_ - Vi) / Ar_);
        Td_ = max(0.0f, -Vr_ / Dr_);
        Tv_ = 0.0f;
    } else {
        // Long move (trapezoidal profile)
        Tv_ = (dX - dXmin) / Vr_;
    }

    // Fill in the rest of the values used at evaluation-time
    Tf_ = Ta_ + Tv_ + Td_;
    Xi_ = Xi;
    Xf_ = Xf;
    Vi_ = Vi;
    yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*sq(Ta_); // pos at end of accel phase

    return true;
}

// # Pass in t as the current time
// # t would be calculated based on milliseconds() passed
// # since the start of the current move
void calc_step_pos_at_arbitrary_time(float t) {
    //Step_t trajStep;
    if (t < 0.0f) {  // Initial Condition
        Y_   = Xi_;
        Yd_  = Vi_;
        Ydd_ = 0.0f;
    } else if (t < Ta_) {  // Accelerating
        Y_   = Xi_ + Vi_*t + 0.5f*Ar_*sq(t);
        Yd_  = Vi_ + Ar_*t;
        Ydd_ = Ar_;
    } else if (t < Ta_ + Tv_) {  // Coasting
        Y_   = yAccel_ + Vr_*(t - Ta_);
        Yd_  = Vr_;
        Ydd_ = 0.0f;
    } else if (t < Tf_) {  // Deceleration
        float td     = t - Tf_;
        Y_   = Xf_ + 0.5f*Dr_*sq(td);
        Yd_  = Dr_*td;
        Ydd_ = Dr_;
    } else if (t >= Tf_) {  // Final Condition
        Y_   = Xf_;
        Yd_  = 0.0f;
        Ydd_ = 0.0f;
    } else {
        // TODO: report error here
    }

    return; // trajStep;
}

And then a utilities file that injects the commander command, parses it, and has the tick method called from the main loop.

g_UTILITIES.ino
//###########################################
// Utilities
// 
// These are extra commands available to help
// use the Dagor controller. They register themselves into the commander
// so when you send ? to this controller in the Serial port, you can
// see these available utilities.
//###########################################

// This init is called from the setup step on boot of ESP32
// Add any registrations of utilities or commander commands here
void util_init() {

  // Add command to commander for torque test. Do this via
  // it's init() method.
  // torque_test_init();

  // Add command to commander for restart
  command.add('r', util_restart_cb, "Restart the ESP32");

  // Add command to commander for break in
  command.add('b', util_breakin_cb, "Break in the motor");

  // Add command to commander for stat
  command.add('S', util_stats_cb, "Stats");

  // Enable toggle
  // command.add('e', util_enable_cb, "Enable/disable toggle");

  // AccelDecel Planner
  command.add('G', util_gcode_move_cb, "Gcode move Gxx, GVxx, or GAxx - Example: G30 moves to position in rads. GV10 sets velocity to 10 rads/s. GA5 sets acceleration to 5 rads/s/s.");

}

// 5 seconds later startup. Add your startup method here. It is called well after all other systems are booted.
void util_startup() {
  // For now, actually run break in so can do this without serial port attached
  //util_breakin_cb("");
  // torque_test_cb("");

  // Start the Gcode utility
  //util_gcode_startup();
  
}

// Add your tick method here for your utility. This is called at 5hz.
bool util_is_did_startup_method = false;
byte util_startup_tick_ctr = 0;

void util_tick() {

  if (util_is_did_startup_method == false) {
    // we have not run our startup routine yet. run it after 10 ticks.
    util_startup_tick_ctr++;

    if (util_startup_tick_ctr >= 20) {
      util_is_did_startup_method = true;
      util_startup_tick_ctr = 0;
      Serial.println("");
      util_startup();
    } else {
      if (util_startup_tick_ctr == 1) {
        Serial.print("Waiting to start utils...");
      } else {
        Serial.print(".");
      }
    }
  }
  
}

// Add your slow tick method here for your utility. This is called at 5hz,
// so use somewhat sparingly.
void util_5hz_tick() {

  // call our own tick method for util startup processing
  util_tick();

  // call your tick method here. will get called at 5hz.
  //breakin_tick();
  // torque_tick();

  // this is the slow tick for repeating the loop over and over for debug of arm
  util_gcode_loop_tick();

}


// Add your super fast tick method here for your utility. This is called as fast as possible,
// so use sparingly.
void util_fast_tick() {
  // util_breakin_micro_tick();
  util_gcode_tick();
}

// Prints out the delta in a nice format of the milliseconds since start to now
// t is time in seconds = millis()/1000;
void timeToString(char* string, size_t size)
{
  unsigned long nowMillis = millis();
  unsigned long seconds = nowMillis / 1000;
  int days = seconds / 86400;
  seconds %= 86400;
  byte hours = seconds / 3600;
  seconds %= 3600;
  byte minutes = seconds / 60;
  seconds %= 60;
  snprintf(string, size, "%04d:%02d:%02d:%02d", days, hours, minutes, seconds);
}

// ---------------------
// AccelDecel Planner
// Gcode Move Gxx Vxx Axx
// Example: G30 V10 A5
// ----------------------
void util_gcode_move_cb(char* cbArg) {
  //Serial.print("Gcode move. cbArg:");
  //Serial.println(cbArg);

  // Parse this string for vals
  if (cbArg[0] == 'V') {
    String str = String(cbArg);

    // Remove v so can convert to a float
    str = str.substring(1);
    // Serial.print("As string removing the v:");
    // Serial.println(str);

    // Now that we have clean string, convert it to a float and set velocity max global variable
    Vmax_ = str.toFloat();
    Serial.print("User wants velocity change. Vmax_: ");
    Serial.println(Vmax_);

    // Try calling the planner to use this new velocity value
    // We have to use the current pos, vel, and accel
    // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
    calc_plan_trapezoidal_path_at_start(Xf_, motor.shaft_angle, motor.shaft_velocity, Vmax_, Amax_, Dmax_);


  }
  else if (cbArg[0] == 'A') {
    String str = String(cbArg);

    // Remove a so can convert to a float
    str = str.substring(1);
    // Serial.print("As string removing the a:");
    // Serial.println(str);

    // Now that we have clean string, convert it to a float and set velocity max global variable
    Amax_ = str.toFloat();
    Dmax_ = Amax_; // For now force decel to be same as accel. At some point add extra command to allow either/or to be set.
    Serial.print("User wants acceleration change. Amax_: ");
    Serial.println(Amax_);

    // Try calling the planner to use this new acceleration value
    // calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
    calc_plan_trapezoidal_path_at_start(Xf_, motor.shaft_angle, motor.shaft_velocity, Vmax_, Amax_, Dmax_);


  }
  else if (cbArg[0] == 'L') {
    // Loop mode
    // This let's you just loop back and forth from one position to another
    util_gcode_loop_start();
  }
  else {
    // Presume they asked for a position move where the cbArg is just a number for the new position
    String str = String(cbArg);
    
    float newPosition = str.toFloat();
    Serial.print("Move to new position (rads): ");
    Serial.println(newPosition);

    // Start the whole Gcode move process
    util_gcode_moveto(newPosition);
  }

}

// This is called by the util startup method that triggers about 5 seconds after the actuator boots up
float util_gcode_loop_posA = 20;
void util_gcode_startup() {
  // Serial.println("Started up AccelDecel Planner Gcode moves");
  Serial.println("Entering endless loop of Gcode moves");
  Vmax_ = 1000;
  Amax_ = 10;

  util_gcode_loop_start();
}

bool is_util_gcode_moving = false;

// -1 off
// 0 want to get going to position A but idle
// 1 going to position A
// 2 want to get going to position B (startPos) but idle
// 3 going to position B (startPos)
// 
int util_gcode_loop_status = -1; 

void util_gcode_loop_start() {

  // tell the status variable that we want to get going to position A
  util_gcode_loop_status = 0;

  // now, next time into the tick() method we'll trigger the moveto

}

void util_gcode_loop_end() {

  // tell the status variable that we want to do nothing
  // that means each time into tick nothing happens, the move will eventually finish, all is good
  util_gcode_loop_status = -1;

  // now, next time into the tick() method we'll trigger nothing
}

void util_gcode_loop_tick() {
  if (util_gcode_loop_status == -1) {
    // user does not want it on, so exit
    return;
  }
  else if (util_gcode_loop_status == 0) {

    // they want to get going to posA
    util_gcode_moveto(util_gcode_loop_posA);
    util_gcode_loop_status = 1; // so next time into tick we know we're executing our move to posA

  }
  else if (util_gcode_loop_status == 1) {

    // we do nothing here while the move is going
    // if the move is done, though, we start moving back to posB (startPos)
    if (is_util_gcode_moving) {
      // they are still moving, do nothing
      return;
    }
    else {
      // they must be done moving,
      util_gcode_loop_status = 2;
    }

  }
  else if (util_gcode_loop_status == 2) {

    // they want to get going to posA
    util_gcode_moveto(0);
    util_gcode_loop_status = 3; // so next time into tick we know we're executing our move to posA

  }
  else if (util_gcode_loop_status == 3) {

    // we do nothing here while the move is going
    // if the move is done, though, we start moving back to posA (endPos)
    if (is_util_gcode_moving) {
      // they are still moving, do nothing
      return;
    }
    else {
      // they must be done moving,
      util_gcode_loop_status = 0;
    }

  }


}

float new_pos_ = 0;
static unsigned long util_gcode_start_move_millis = 0;

// When ready to move to new position, call this with the new position
void util_gcode_moveto(float newPos) {

  // See if already moving. Exit if so.
  // if (is_util_gcode_moving) {
  //   Serial.println("Already in a move. Cancelling request to move to new position.");
  //   return;
  // }

  // set our global of the new position
  Xf_ = newPos;

  // set our milliseconds passed variable so we have a t to work with
  util_gcode_start_move_millis = millis();

  // take the position from SimpleFOC and set it to our start position
  Xi_ = motor.shaft_angle;
  Serial.print("Starting at shaft angle of:");
  Serial.println(Xi_);

  // TODO: If we are being asked to move but are already in motion, we should go with the current velocity, position, and acceleration
  // and keep moving.

  // Now we need to do our calcs before we start moving
  // TODO: Maybe stop passing vars and just use all globals
  calc_plan_trapezoidal_path_at_start(Xf_, Xi_, Vi_, Vmax_, Amax_, Dmax_);
  motor.target = Y_; // could possibly put this in the method above

  // Tell user how long this move will take
  Serial.print("Time to complete move (secs):");
  Serial.println(Tf_);

  // Velocity and Accel of move
  Serial.print("Velocity for move: ");
  Serial.print(Vmax_);
  Serial.print(", Acceleration: ");
  Serial.println(Amax_);

  // set our global bool so the tick method knows we have a move in motion
  is_util_gcode_moving = true;

  // motor.monitor_downsample = 250; // enable monitor every 250ms

}

// This should get call at 100hz
unsigned int gcode_step_ctr = 0;
static unsigned long lastTimeUpdateMillis = 0;

int util_gcode_period = 10; // 1000 / this number = Hz, i.e. 1000 / 100 = 10Hz, 1000 / 10 = 100Hz, 1000 / 5 = 200Hz, 1000 / 1 = 1000hZ
unsigned long util_gcode_time_now = 0;

void util_gcode_tick() {

  // This should get entered 100 times each second (100Hz)
  if((unsigned long)(millis() - util_gcode_time_now) > util_gcode_period){
    util_gcode_time_now = millis();
    //Serial.print(".");

    // see if we are in a move or not 
    if (is_util_gcode_moving) {
      // we are in a move, let's calc the next position
      float secs_since_start_of_move = (millis() - util_gcode_start_move_millis) / 1000.0f;
      //Serial.println(millis_since_start_of_move);
      calc_step_pos_at_arbitrary_time(secs_since_start_of_move);
      motor.target = Y_;

      // Serial.print("t (secs): ");
      // Serial.print(secs_since_start_of_move);
      // Serial.print(", Y_ (rads): ");
      // Serial.println(Y_);

      // Called 4 times per second
      /**
      gcode_step_ctr++;
      if (gcode_step_ctr >= 50) {
        Serial.print(secs_since_start_of_move);
        Serial.print(", ");
        util_stats();
        gcode_step_ctr = 0;
      }
      **/

      // see if we are done with our move
      if (secs_since_start_of_move >= Tf_) {
        // we are done with move
        // motor.monitor_downsample = 0; // disable monitor
        Serial.println("Done with move");
        is_util_gcode_moving = false;
      }

    }

    // // Called once/second
    // gcode_step_ctr++;
    // if (gcode_step_ctr >= 25) {
    //   char str[20] = "";
    //   timeToString(str, sizeof(str));
    //   Serial.print(str);
    //   gcode_step_ctr = 0;
    //   util_stats();
    // }

  }

}

// ----------------
// ENABLE TOGGLE
// ----------------

void util_enable_cb(char* cbArg) {
  Serial.print("Toggling enable. cbArg:");
  Serial.println(cbArg);

  util_enable_toggle();

}


void util_enable_toggle() {
  int gate_val = digitalRead(enGate);
  if (gate_val == HIGH) {
    digitalWrite(enGate, LOW);
    Serial.println("Toggled enable to off.");
  } else {
    digitalWrite(enGate, HIGH);
    Serial.println("Toggled enable to on.");
  }
}

// --------------
// RESTART
// ----------------

// The callback when user enters command at serial port. This restarts the ESP32.
void util_restart_cb(char* cbArg) {
  Serial.print("Restarting ESP32. cbArg:");
  Serial.println(cbArg);

  ESP.restart();
  

}

// --------------
// BREAK IN MOTOR
// ----------------

bool breakin_is_running = false;
unsigned int breakin_tick_ctr = 0;
int breakin_vel_current = 0;
int breakin_vel_start = 1;
int breakin_vel_increment = 1;
int breakin_vel_end = 65; //125;
byte breakin_phase = 0; // 0:stop, 1:fwd accel to end, 2:fwd decel back to 0, 3:rev accel to -end, 4: rev decel back to 0
unsigned int util_breakin_stateT = 0;

// The callback when user enters command at serial port. This triggers the break in function.
void util_breakin_cb(char* cbArg) {
  
  Serial.print("Toggling break in of motor. cbArg:");
  Serial.print(cbArg);

  if (breakin_is_running) {
    Serial.println("Toggling OFF");
    breakin_finish();
  }
  else {
    //Serial.println("Starting break in of motor.");
    Serial.println("Toggling ON");
    Serial.println("We will rotate at differing velocities and in different directions");
    Serial.println("in an unending loop. Enter 'b' again to toggle the break in off.");
    Serial.print("We will accel/decel to +/- velocity of: ");
    Serial.println(breakin_vel_end);
    Serial.print("At a step increment of: ");
    Serial.println(breakin_vel_increment);


    breakin_begin();
  }

}

void breakin_begin() {

  // ok, now do move where we go to start position, pause for 2 seconds, then move back
  breakin_tick_ctr = 0;
  breakin_is_running = true;
  //breakin_fwd_dir = true;
  breakin_phase = 1; // 1:fwd accel to end

  // make sure we are in velocity mode

  breakin_vel_current = breakin_vel_start; // + torque_increment;
  motor.target = breakin_vel_current;
  motor.move();
  
  //Serial.print("Setting velocity to: ");
  //Serial.println(motor.target);
  //Serial.println("The micro tick callback will execute at 25 hertz");
  Serial.println("Going to phase 1:fwd accel to end");
  util_stats();
}

void breakin_step() {

  // increment velocity
  breakin_vel_current = breakin_vel_current + breakin_vel_increment;

  // see what phase we're in
  // 0:stop, 1:fwd accel to end, 2:fwd decel to 0, then rev accel to -end, 3:rev decel back to 0
  if (breakin_phase == 1) {
    
    // see if we're at end
    if (breakin_vel_current > breakin_vel_end) {
           
      // if we get here, we're at end of forward direction, so now we need to go reverse direction
      // meaning slow down
      breakin_vel_increment = -1 * breakin_vel_increment;
      breakin_vel_end = -1 * breakin_vel_end;
      breakin_phase = 2; // skip to 2:rev accel to -end
      Serial.println("Going to phase 2:fwd decel to 0, then rev accel to -end");
      util_stats();
            
    } else {
      // i think do nothing for now
    }
  }
  else if (breakin_phase == 2) {
    
    // see if we're at -end
    if (breakin_vel_current < breakin_vel_end) {
           
      // if we get here, we're at end of reverse direction to -end, so now we need to go fwd direction
      // meaning slow down back to 0
      breakin_vel_increment = -1 * breakin_vel_increment;
      breakin_vel_end = -1 * breakin_vel_end;
      breakin_phase = 3;
      Serial.println("Going to phase 3:rev decel back to 0");
      util_stats();
            
    } else {
      // i think do nothing for now
    }
  }
  else if (breakin_phase == 3) {
    
    if (breakin_vel_current > 0) {
      // This means we slowed all the way down to 0 so can end  
      Serial.println("Going back to phase 1.");
      util_stats();

      // Let's start all over again
      breakin_begin(); 
      
      //breakin_finish();
      return;   
    }
    
  }
  else {
    Serial.print("Got phase that should not get. breakin_phase:");
    Serial.println(breakin_phase);
  }
  
  motor.target = breakin_vel_current;
  motor.move();
  //Serial.println("Going to new velocity for break in of motor.");
  //Serial.print("Setting velocity to: ");
  //Serial.println(motor.target);
}

void breakin_finish() {

  // done with breakin, so set bool that we're done
  breakin_is_running = false;
  breakin_vel_increment = abs(breakin_vel_increment);
  breakin_vel_end = abs(breakin_vel_end);
  breakin_phase = 0;
    
  breakin_vel_current = -1;
  motor.target = 0;
  motor.move();
  Serial.println("Ending break in of motor.");
  //Serial.print("Moving to: ");
  //Serial.println(motor.target);
  util_stats();
}


void util_breakin_micro_tick() {
  
  util_breakin_stateT++;

  // Functions inside this "if" will execute at a 50hz rate (the denominator)
  if(util_breakin_stateT >= 100000/50){
    util_breakin_stateT = 0;
    breakin_tick();
    //breakin_step();
  }

}

// this is called at 5hz, that way we can trigger off of it
void breakin_tick() {
  
  if (breakin_is_running) {

    // At 2 seconds
    //if (breakin_tick_ctr == 10) {
    //  torque_tick_ctr++;
    //  torque_relax();
    //}
    
    // Each time in here (50Hz)
    breakin_step();

    // At 1 second
    if (breakin_tick_ctr >= 10) {
  
      //Serial.println("breakin_tick_ctr was 15, so doing next step...");
      breakin_tick_ctr = 0;
      util_stats();
      
    } 
    else {
      breakin_tick_ctr++;

    }
    
  } 
  
}

// --------------
// STATS
// ----------------
/**
// current target value
float target;
// current motor angle
float shaft_angle;
// current motor velocity 
float shaft_velocity;
// current target velocity
float shaft_velocity_sp;
// current target angle
float shaft_angle_sp;

// current voltage set to the motor (voltage.q, voltage.d)
DQVoltage_s voltage;
// current (if) measured on the motor (current.q, current.d)
DQCurrent_s current;
// phase voltages 
float Ua, Ub, Uc;
**/
void util_stats() {
  Serial.print("Angle: ");
  Serial.print(motor.shaft_angle);
  Serial.print(", Velocity: ");
  Serial.print(motor.shaft_velocity);
  Serial.print(", Voltage q: ");
  Serial.print(motor.voltage.q);
  Serial.print(", Temp: ");
  Serial.println(util_temp(), 2);
}

// The callback when user enters command at serial port. This restarts the ESP32.
void util_stats_cb(char* cbArg) {
  //Serial.print("Stats. cbArg:");
  //Serial.println(cbArg);

  util_stats();

}

float util_temp() {

  float vOut = 0;
  for (byte ctr = 0; ctr < 10; ctr++) {
    vOut += analogRead(vTemp);
  
  }
  return ((((vOut/10)*3.3)/4095)-1.8577)/-0.01177;
  
}

// --------------
// TORQUE TEST
// ----------------

// Setup torque test
float torque_pre_start = 0;
float torque_start = 10;
float torque_end = 50;
float torque_increment = 5;
float torque_current = -1; // stores the current move to target
bool torque_is_testing = false;
int torque_tick_ctr = 0;

void torque_test_init() {
  // add command to commander so we can do torque test
  command.add('t', torque_test_cb, "Torque Test");
}

// The callback when user enters command at serial port. This starts test.
void torque_test_cb(char* cbArg) {
  Serial.print("torque_test_cb. cbArg:");
  Serial.println(cbArg);

  torque_begin();

}

void torque_begin() {

  // ok, now do move where we go to start position, pause for 2 seconds, then move back
  torque_tick_ctr = 0;
  torque_is_testing = true;

  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  //motor.PID_velocity.output_ramp = 1;

  // setting the limits
  //  maximal velocity of the position control
  motor.velocity_limit = 0.001; // rad/s - default 20

  // acceleration control using output ramp
  // this variable is in rad/s^2 and sets the limit of acceleration
  motor.P_angle.output_ramp = 10; // default 1e6 rad/s^2

  // angle P controller -  default P=20
  motor.P_angle.P = 0.5;

  torque_current = torque_start; // + torque_increment;
  motor.target = torque_current;
  motor.move();
  Serial.println("Starting torque test.");
  Serial.print("Moving to: ");
  Serial.println(motor.target);
}

// in this step we go back to the start number quickly, like after 2 ticks
// so that we don't put too much pressure on the mosfets or current draw
void torque_relax() {
  motor.target = torque_pre_start;
  motor.move();
  Serial.println("Back to relax position.");
  Serial.print("Moving to: ");
  Serial.println(motor.target);
}

void torque_step() {
  torque_current = torque_current + torque_increment;

  // see if we're at end
  if (torque_current > torque_end) {
    torque_finish();
    return;
  }
  
  motor.target = torque_current;
  motor.move();
  Serial.println("Next step on torque test.");
  Serial.print("Moving to: ");
  Serial.println(motor.target);
}

void torque_finish() {

  // done testing at 2 seconds, so go back to start position
  torque_is_testing = false;
    
  torque_current = -1;
  motor.target = torque_pre_start;
  motor.move();

  Serial.println("Ending torque test.");
  Serial.print("Moving to: ");
  Serial.println(motor.target);

  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  //motor.PID_velocity.output_ramp = 1000;

  // setting the limits
  //  maximal velocity of the position control
  //motor.velocity_limit = 20; // rad/s - default 20

  torque_begin();
}

// this is called at 5hz, that way we can trigger off of it
void torque_tick() {
  
  if (torque_is_testing) {

    if (torque_tick_ctr == 50) {
      torque_tick_ctr++;
      torque_relax();
    }
    else if (torque_tick_ctr == 100) {
  
      Serial.println("torque_tick_ctr was 15, so doing next step...");
      torque_tick_ctr = 0;
      torque_step();
      
    } 
    else {
      torque_tick_ctr++;

    }
    
  } 
  
}

Enjoy.

3 Likes

Awesome project! I just gotta say, this inspired me to se if I can ditch the steppers on my pnp and do some sort of 3D printed gearing for a sensored gimbal. How fast can those motors spin @24v ?

Amazing … I will try it very soon :slight_smile:

Gimbal motors have KVs in the range 50-200, I’d say, so 1000-4000 RPM @24V
But I personally wouldn’t aim to drive them near their max. Their high Ohm windings typically have quite thin wires and in my experience they get very hot if you give them a lot of current for any length of time…

Ok, thanks. That is ideal for pnp. I’m thinking sensored bldc for eboard with a reduktion gearing. Even 1000-1500rpm with a reduktion gearing would be much faster then a nema23 stepper. I would drive it way below rated watt. Maybe 250w. I’m contemplating if it is possible to drive the bldc using the inbuilt hall’s and use the analog hall encoder I’m working on, mounted on the reduction gear next to the bldc… I see it’s possible to get eboard motor/pulley belt mounts.