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

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:





3 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.

1 Like

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)
2 Likes

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.

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.

Aha, that’s what I’m talking about! 250w Odrive XY lightplacer.

http://cyberspaceandtime.com/FUh36RUHzdU.video+related

@jlauer

Is it possible to place latching hall sensors on the ML5010 type stator? Are U using the encoder on the dagor to commutate the motor? Maybe someone else has experience with mounting hall sensors to a stator. Will it work to simply glue the sensors on top of the stator, close to the edge?

I see these motors has a 850w max continues rating, so driving them @ 250-350w should keep them cool, right? I assume they have enough torque to drive a XY pnp robot in FOC mode?

I’m just using the diametric magnet and the AS5147 sensor on the Dagor board. Why mess around with hall sensors anymore? Your time and money on that will be more expensive than the $6 AS5147 and $0.25 magnet.

2 Likes

Ok I see, I would have a encoder on the reduction gear and use the hall sensors to commutate the motor. Another way would be to use a sensored bldc, but this would be even more expensive.

No, I would commutate with an encoder these days. Then, if you want another encoder after the reduction gears, just use a 2nd encoder. I think SimpleFOC does a good job keeping track of the motor position without needing an encoder on the other side of the reduction gears. However, after power off, it is a challenge to know the real position of device after the reduction gears. On my robot arm, I’m just going to rely on the user to always place the arm back into a rough home position to achieve the 0 position after power off.

I have to say, I’m with @jlauer on this one… I mean, unless you want to undertake the hall-sensor self-made encoder project for its own sake…
But I think you will find it quite a challenge. The encoder ICs have many hall elements, and sophisticated filtering to achieve the accuracy they get from a very close diametrical magnet. Directly sensing the rotor magnets from the side will be challenging, and I think you’ll need quite a bit of filtering (perhaps a DSP?) afterwards to get a useful signal. But maybe I don’t fully understand the concept…
So if the goal is motor commutation, I’d definately go for a pre-made sensor… if the goal is learning about magnetic sensing, then your way is much better :smiley:

You both have good point´s. My main goal is to drive high current LED´s without limiting resistors :smiley: I know, i know… its for another forum. The motor part is more or less a bonus and I am in it to hopefully develop my PnP machine. To drive steppers like 2 phase BLDC´s would be a great achievement and I believe there could possibly be a way to do that with the homebrewed encoder. There is alot of focus on steppers around openPnP, but as you see in the link I posted earlier, definitely eboard outrunners can do a very nice (fast) job. They do come at a higher price point, but at the end of the day, sometimes you have to pay for the torque at higher speed. But who says it has to be the cheapest possible solution. There are infact folks building high end Pnp´s around openPnp. Good thing the eboard is on the rise, and prices on those motors are going down. I will try some sensored BLDC´s and use a encoder for the precision work. If the DIY encoder fails, I will most likely fall back on the IC encoder in a breakout format.

https://flipsky.net/collections/e-skateboard/products/bldc-motor-h6355-160kv-1620w?variant=29532346351676

Is the motion planner ready for SimpleFoc library? (accel/ decel/ trapezoidal movement)

were you able to move this motor? I have a similar motor but it makes a lot of noise.