OpenPnP w. sFOC


From the get go, my initial reason for attempting to make a motor controller was my interest in OpenPnP.

Now I have a working sFOC stepper setup on my desc, and think the time is right for trying to run OpenPnP with sFOC.

Plz feel free to Download OpenPnP and join the effort. If you do not have experience with OpenPnP it can be a real learning experience in Machine Vision and Motion.

I have taken a quick look at OpenPnP (havent seen it in a while) and there are several motion control types to choose between. Firstly I will go for what is possible with the trapezoidal-planner @JorgeMaker. How does the planner react, when receiving a command during a move. How can we implement a queue?

When setting up a motor_driver in OpenPnP, from scratch, you more or less has to assign your own commands. Like move_done, turn on valve etc.

With the current planner code, it should therefore be pretty straightforward to connect the two, plus create some more advanced motion profiles later on.

OpenPnP – Open Source SMT Pick and Place

1 Like

Quick Start · openpnp/openpnp Wiki · GitHub

Motion Controller Firmwares · openpnp/openpnp Wiki · GitHub

How cool would it be to have sFOC on that page !

I guess we need a queue for the more advanced stuff. Since OpenPNP is able to send like a string of commands with lots of moves on a string. Unless the parameters are dynamic, then it can just send the moves which is stored in a buffer? Right, buffer, we need the Serial.buffer.

We can really send any command on the fly, like torque, acceleration etc.

We definately need some Mcommands:

// Parse this string for vals
    String commandSrt = String(gCodeCommand);
    float commandValue;
    switch (gCodeCommand[0]){

        case 'M':
        // Remove V so can convert to a float
        commandSrt = commandSrt.substring(1);
        commandValue = commandSrt.toFloat();
        M_command= commandValue;

        // Place different if statements like "fetch current position".

        #ifdef __debug
            Serial.print("M Command :  ");

I think G should have a sub class with G0 G1 moves.

Hi @Juan-Antonio_Soren_E,

A very interesting project. It has been a while since I last implemented the trapezoidal planner, but from what I can remember, when the planner receives a new command, it discontinues the current motion and initiates the execution of the new one.

If you desire a queue of trajectories you would need to incorporate it within the planner or on top of it ensuring that it stores movements in a queue until the current one is finished. Adding such functionality should likely not pose a significant implementation challeng.

regards !!

  commander.add('G', doPlanner, "Motion Planner");
  commander.add('M', MPlanner, "Motion Planner");

void MPlanner(char *cmd){
void TrapezoidalPlanner::doMCommand(char *MCommand){
  #ifdef __debug
        Serial.print("GGode command: M");

     String commandSrt = String(MCommand);
    float commandValue;
    switch (MCommand[0]){

       case 'M':
        // Remove M so can convert to a float
        commandSrt = commandSrt.substring(1);
        commandValue = commandSrt.toFloat();
        float M_command = 0.0f;
        M_command = commandValue;

        //if M_command = 

        // Place different if statements like "fetch current position".

        #ifdef __debug
            Serial.print("M Command :  ");


Ok, copilot just made this system report ?

        if (M_command == 115){
            // M115
            // Send firmware version and capabilities

 if (M_command == 114){

        // M114
        // Send current position

Note: convert to mm for openPnP

// The controller must be able to set the current position, typically with the M206 command. This is used to set the current position to 0, for example, when homing.

            if (M_command == 206){
                // M206
                // Set current position

                //  TODO  TODO TODO TODO
        //  TODO  TODO TODO TODO


            if (M_command == 203){
                // M203
                // Set maximum feedrate

                //  TODO  TODO TODO TODO
        //  TODO  TODO TODO TODO


            if (M_command == 201){
                // M201
                // Set maximum acceleration

                //  TODO  TODO TODO TODO
        //  TODO  TODO TODO TODO

            if (M_command == 202){
                // M202
                // Set maximum jerk

                //  TODO  TODO TODO TODO
        //  TODO  TODO TODO TODO

            if (M_command == 205){
                // M205
                // Advanced Settings

                //  TODO  TODO TODO TODO
        //  TODO  TODO TODO TODO


change offset angle ?

My advice: try to build your functionality on top of the planner creating a queue of movements and execute them sequentially waiting for the ending of the current movement before launching a new movement.

1 Like

Right now im just trying to convert from radians to mm. I suppose we need a mm/rev variable.

        float mm_per_rev = 17.357f;
        float angle_rapport = motor->shaft_angle;
        //convert to mm from 2 x radians per revolution using the mm_per_rev variable as the mm per 
        float mm =  map(angle_rapport, 0, 2*PI, 0, mm_per_rev);
        Serial.println(mm, 8);

Ok, got something working here.

if (commandValue_Int == 206){
                // M206
                // Set current position, for homing to 0. 

                commandSrt = commandSrt.substring(4);
                commandValue2 = commandSrt.toFloat();
                motor->shaft_angle = commandValue2;

                Serial.print("Homing to: ");

                Serial.print("Chekking shaft_angle : ");



GGode command: M206 2.37

Homing to: 2.37
Chekking shaft_angle : 2.37
M Command :  206

Yes, that makes sense if using M400

  1. The controller must be able to wait for motion completion, typically with the M400 command. Any further commands sent after the M400 must be suspended until motion completion. The controller must only acknowledge the command, when motion is complete i.e. the “ok” (COMMAND_CONFIRM_REGEX) response must be suspended until then, providing blocking synchronization to OpenPnP.

Actually, when you use a stepper motor, you plan_ahead how many step-pulses you have to send, to finish a move. Based on that number you can always tell when a move has finished, without using M400.
The planner buffer is different from the Serial buffer.
It holds step-rates based on the F-parameter in Gx commands and step-pulses (with direction) for each motor involved.

I think you missed something. SFOC does not step the stepper in a traditional sense. The M400 command is specified by OpenPNP, as being important for advanced motion types.


chatGBT has suggested this circular buffer. I suppose, this should actually be part of the commander, since it is the one reading the Serial.buffer?

#define COMMAND_BUFFER_SIZE 32 // Adjust the size as needed
char commandBuffer[COMMAND_BUFFER_SIZE][128]; // Circular buffer to store commands
int bufferStart = 0; // Index where new commands are added
int bufferEnd = 0;   // Index where executed commands are removed
 // Check for incoming serial data
    while (Serial.available() > 0) {
        char c =;
        // Check for command termination ('\n' or '\r') to process the command
        if (c == '\n' || c == '\r') {
            // Null-terminate the current command to make it a valid C string
            commandBuffer[bufferStart][commandBufferIndex] = '\0';
            // Move the bufferStart index to the next position in the circular buffer
            bufferStart = (bufferStart + 1) % COMMAND_BUFFER_SIZE;
            // Process the command from the circular buffer
            // Move the bufferEnd index to the next position
            bufferEnd = (bufferEnd + 1) % COMMAND_BUFFER_SIZE;
            // Clear the buffer for the next command
            commandBufferIndex = 0;
        } else {
            // Add the character to the current buffer position
            if (commandBufferIndex < sizeof(commandBuffer[0]) - 1) {
                commandBuffer[bufferStart][commandBufferIndex] = c;
            } else {
                // Handle buffer overflow error
                // You can add error handling code here

    // Other loop code

Maybe we should set a flag called move_done?

This is the commander.cpp function, nothing circular about it;

void Commander::run(Stream& serial, char eol){
  Stream* tmp = com_port; // save the serial instance
  char eol_tmp = this->eol;
  this->eol = eol;
  com_port = &serial;

  // a string to hold incoming data
  while (serial.available()) {
    // get the new byte:
    int ch =;
    received_chars[rec_cnt++] = (char)ch;
    // end of user input
    if (isSentinel(ch)) {
      // execute the user command

      // reset the command buffer
      received_chars[0] = 0;
    if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
        received_chars[0] = 0;

Meybe it should be a different class, like gCodeParserBuffer or something?


bool Commander::isSentinel(char ch)
  if(ch == eol)
    return true;
  else if (ch == '\r')
      printVerbose(F("Warn: \\r detected! \n"));
  return false;

I can not say. Too long ago since I developed such code. I am quite busy now with a new job and don’t have too spare time to side projects.

have you tried step-dir interface with your boards?
@Copper280z seemed to get really great results on 3d printer- may work also on PnP just using the regular stepper scheduler.

We are merging to code platforms. Or rather implementing Klipper support for SimpleFOC. SimpleFOC is a open-source software repository for driving varius motors
BLDC, steppers etc, using Field Oriented Control. Recently I have been working on a NEMA23 stepper hardware design, using the integrated MCU, BUCK and FET driver + 
1 additional FET driver for 8PWM control of 4 halfbridges. The hardware is so far working with the SimpleFOC repo and included trepazoidalplanner. 

What we wich to achieve, is closed loop control of a multiaxial machine, where the individual motor controller is running SimpleFOC, recieving and executing commands,
in a timely manner, as is the Klipper protocol trademark. The USB connection should be made possible for the STM32G4 (STSPIN32G4)

Klipper is a gCode enterpretor and MCU / Motion mainframe. It uses true USB coms and therefore use the capabilities of the USB package, which is more ideal
compared to single char Serial Coms. Klipper is able to sync varius sub_drivers in order to control a multi-axis machine, from a gCode. We want the hardware,
I have designed (The Field Stack), to work with klipper commands, running SimpleFOC (Field Oriented Stepper control). 

1. Utilize Klipper protocol accelerating, cruise and de-accelerating feature, when designing the SFOC Klipper-command parser and executer. 

2. Utilize that we have closed loop control using the 16bit hardware encoder counter, and a very precise and calibrated MT6835 angle sensor connected with ABZ. 
Note: Find a way to implement the Z puls, if it is not already in use.

3. Stay true to already existing syntax.

Both SimpleFOC with varius subfolder and Klipper is included in this directory. 

We also want a txt file with all the changes made and simple to understand, not too long, explainations.

git clone GitHub - Klipper3d/klipper: Klipper is a 3d-printer firmware