try to fix submodule

firmware/lib/Arduino-FOC/src/communication/Commander.cpp

@@ -0,0 +1,685 @@
+#include "Commander.h"
+
+Commander::Commander(Stream& serial, char eol, bool echo){
+  com_port = &serial;
+  this->eol = eol;
+  this->echo = echo;
+}
+Commander::Commander(char eol, bool echo){
+  this->eol = eol;
+  this->echo = echo;
+}
+
+
+void Commander::add(char id, CommandCallback onCommand, char* label ){
+  call_list[call_count] = onCommand;
+  call_ids[call_count] = id;
+  call_label[call_count] = label;
+  call_count++;
+}
+
+
+void Commander::run(){
+  if(!com_port) return;
+  run(*com_port, eol);
+}
+
+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 = serial.read();
+    received_chars[rec_cnt++] = (char)ch;
+    // end of user input
+    if(echo)
+      print((char)ch);
+    if (isSentinel(ch)) {
+      // execute the user command
+      run(received_chars);
+
+      // reset the command buffer
+      received_chars[0] = 0;
+      rec_cnt=0;
+    }
+    if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
+        received_chars[0] = 0;
+        rec_cnt=0;
+    }
+  }
+
+  com_port = tmp; // reset the instance to the internal value
+  this->eol = eol_tmp;
+}
+
+void Commander::run(char* user_input){
+  // execute the user command
+  char id = user_input[0];
+  switch(id){
+    case CMD_SCAN:
+      for(int i=0; i < call_count; i++){
+          printMachineReadable(CMD_SCAN);
+          print(call_ids[i]);
+          print(":");
+          if(call_label[i]) println(call_label[i]);
+          else println("");
+      }
+      break;
+    case CMD_VERBOSE:
+      if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
+      printVerbose(F("Verb:"));
+      printMachineReadable(CMD_VERBOSE);
+      switch (verbose){
+      case VerboseMode::nothing:
+        println(F("off!"));
+        break;
+      case VerboseMode::on_request:
+      case VerboseMode::user_friendly:
+        println(F("on!"));
+        break;
+      case VerboseMode::machine_readable:
+        printlnMachineReadable(F("machine"));
+        break;
+      }
+      break;
+    case CMD_DECIMAL:
+      if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
+      printVerbose(F("Decimal:"));
+      printMachineReadable(CMD_DECIMAL);
+      println(decimal_places);
+      break;
+    default:
+      for(int i=0; i < call_count; i++){
+        if(id == call_ids[i]){
+          printMachineReadable(user_input[0]);
+          call_list[i](&user_input[1]);
+          break;
+        }
+      }
+      break;
+  }
+}
+
+void Commander::motor(FOCMotor* motor, char* user_command) {
+
+  // if target setting
+  if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+' || isSentinel(user_command[0])){
+    target(motor, user_command);
+    return;
+  }
+
+  // parse command letter
+  char cmd = user_command[0];
+  char sub_cmd = user_command[1];
+  // check if there is a subcommand or not
+  int value_index = (sub_cmd >= 'A'  && sub_cmd <= 'Z') ||  (sub_cmd == '#') ?  2 :  1;
+  // check if get command
+  bool GET = isSentinel(user_command[value_index]);
+  // parse command values
+  float value = atof(&user_command[value_index]);
+  printMachineReadable(cmd);
+  if (sub_cmd >= 'A'  && sub_cmd <= 'Z') {
+    printMachineReadable(sub_cmd);
+  }
+
+  // a bit of optimisation of variable memory for Arduino UNO (atmega328)
+  switch(cmd){
+    case CMD_C_Q_PID:      //
+      printVerbose(F("PID curr q| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]);
+      else pid(&motor->PID_current_q,&user_command[1]);
+      break;
+    case CMD_C_D_PID:      //
+      printVerbose(F("PID curr d| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]);
+      else pid(&motor->PID_current_d, &user_command[1]);
+      break;
+    case CMD_V_PID:      //
+      printVerbose(F("PID vel| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]);
+      else pid(&motor->PID_velocity, &user_command[1]);
+      break;
+    case CMD_A_PID:      //
+      printVerbose(F("PID angle| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]);
+      else pid(&motor->P_angle, &user_command[1]);
+      break;
+    case CMD_LIMITS:      //
+     printVerbose(F("Limits| "));
+      switch (sub_cmd){
+        case SCMD_LIM_VOLT:      // voltage limit change
+          printVerbose(F("volt: "));
+          if(!GET) {
+            motor->voltage_limit = value;
+            motor->PID_current_d.limit = value;
+            motor->PID_current_q.limit = value;
+            // change velocity pid limit if in voltage mode and no phase resistance set
+            if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
+          }
+          println(motor->voltage_limit);
+          break;
+        case SCMD_LIM_CURR:      // current limit
+          printVerbose(F("curr: "));
+          if(!GET){
+            motor->current_limit = value;
+            // if phase resistance specified or the current control is on set the current limit to the velocity PID
+            if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
+          }
+          println(motor->current_limit);
+          break;
+        case SCMD_LIM_VEL:      // velocity limit
+          printVerbose(F("vel: "));
+          if(!GET){
+            motor->velocity_limit = value;
+            motor->P_angle.limit = value;
+          }
+          println(motor->velocity_limit);
+          break;
+        default:
+          printError();
+          break;
+      }
+      break;
+    case CMD_MOTION_TYPE:
+    case CMD_TORQUE_TYPE:
+    case CMD_STATUS:
+      motion(motor, &user_command[0]);
+      break;
+    case CMD_PWMMOD:
+       // PWM modulation change
+       printVerbose(F("PWM Mod | "));
+       switch (sub_cmd){
+        case SCMD_PWMMOD_TYPE:      // zero offset
+          printVerbose(F("type: "));
+          if(!GET) motor->foc_modulation = (FOCModulationType)value;
+          switch(motor->foc_modulation){
+            case FOCModulationType::SinePWM:
+              println(F("SinePWM"));
+              break;
+            case FOCModulationType::SpaceVectorPWM:
+              println(F("SVPWM"));
+              break;
+            case FOCModulationType::Trapezoid_120:
+              println(F("Trap 120"));
+              break;
+            case FOCModulationType::Trapezoid_150:
+              println(F("Trap 150"));
+              break;
+          }
+          break;
+        case SCMD_PWMMOD_CENTER:      // centered modulation
+          printVerbose(F("center: "));
+          if(!GET) motor->modulation_centered = value;
+          println(motor->modulation_centered);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_RESIST:
+      printVerbose(F("R phase: "));
+      if(!GET){
+        motor->phase_resistance = value;
+        if(motor->torque_controller==TorqueControlType::voltage)
+          motor->PID_velocity.limit= motor->current_limit;
+      }
+      if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
+      else println(0);
+      break;
+    case CMD_INDUCTANCE:
+      printVerbose(F("L phase: "));
+      if(!GET){
+        motor->phase_inductance = value;
+      }
+      if(_isset(motor->phase_inductance)) println(motor->phase_inductance);
+      else println(0);
+      break;
+    case CMD_KV_RATING:
+      printVerbose(F("Motor KV: "));
+      if(!GET){
+        motor->KV_rating = value;
+      }
+      if(_isset(motor->KV_rating)) println(motor->KV_rating);
+      else println(0);
+      break;
+    case CMD_SENSOR:
+      // Sensor zero offset
+       printVerbose(F("Sensor | "));
+       switch (sub_cmd){
+        case SCMD_SENS_MECH_OFFSET:      // zero offset
+          printVerbose(F("offset: "));
+          if(!GET) motor->sensor_offset = value;
+          println(motor->sensor_offset);
+          break;
+        case SCMD_SENS_ELEC_OFFSET:      // electrical zero offset - not suggested to touch
+          printVerbose(F("el. offset: "));
+          if(!GET) motor->zero_electric_angle = value;
+          println(motor->zero_electric_angle);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_MONITOR:     // get current values of the state variables
+      printVerbose(F("Monitor | "));
+      switch (sub_cmd){
+        case SCMD_GET:      // get command
+          switch((uint8_t)value){
+            case 0: // get target
+              printVerbose(F("target: "));
+              println(motor->target);
+              break;
+            case 1: // get voltage q
+              printVerbose(F("Vq: "));
+              println(motor->voltage.q);
+              break;
+            case 2: // get voltage d
+              printVerbose(F("Vd: "));
+              println(motor->voltage.d);
+              break;
+            case 3: // get current q
+              printVerbose(F("Cq: "));
+              println(motor->current.q);
+              break;
+            case 4: // get current d
+              printVerbose(F("Cd: "));
+              println(motor->current.d);
+              break;
+            case 5: // get velocity
+              printVerbose(F("vel: "));
+              println(motor->shaft_velocity);
+              break;
+            case 6: // get angle
+              printVerbose(F("angle: "));
+              println(motor->shaft_angle);
+              break;
+            case 7: // get all states
+              printVerbose(F("all: "));
+              print(motor->target);
+              print(";");
+              print(motor->voltage.q);
+              print(";");
+              print(motor->voltage.d);
+              print(";");
+              print(motor->current.q);
+              print(";");
+              print(motor->current.d);
+              print(";");
+              print(motor->shaft_velocity);
+              print(";");
+              println(motor->shaft_angle);
+              break;
+            default:
+              printError();
+              break;
+          }
+          break;
+        case SCMD_DOWNSAMPLE:
+          printVerbose(F("downsample: "));
+          if(!GET) motor->monitor_downsample = value;
+          println((int)motor->monitor_downsample);
+          break;
+        case SCMD_CLEAR:
+          motor->monitor_variables = (uint8_t) 0;
+          println(F("clear"));
+          break;
+        case CMD_DECIMAL:
+          printVerbose(F("decimal: "));
+          motor->monitor_decimals = value;
+          println((int)motor->monitor_decimals);
+          break;
+        case SCMD_SET:
+          if(!GET){
+            // set the variables
+            motor->monitor_variables = (uint8_t) 0;
+            for(int i = 0; i < 7; i++){
+              if(isSentinel(user_command[value_index+i])) break;
+              motor->monitor_variables |=  (user_command[value_index+i] - '0') << (6-i);
+            }
+          }
+          // print the variables
+          for(int i = 0; i < 7; i++){
+            print( (motor->monitor_variables & (1 << (6-i))) >> (6-i));
+          }
+          println("");
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    default:  // unknown cmd
+      printVerbose(F("unknown cmd "));
+      printError();
+  }
+}
+
+void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
+  char cmd = user_cmd[0];
+  char sub_cmd = user_cmd[1];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[(sub_cmd >= 'A'  && sub_cmd <= 'Z') ?  2 :  1]);
+
+  switch(cmd){
+    case CMD_MOTION_TYPE:
+      printVerbose(F("Motion:"));
+      switch(sub_cmd){
+        case SCMD_DOWNSAMPLE:
+            printVerbose(F(" downsample: "));
+            if(!GET) motor->motion_downsample = value;
+            println((int)motor->motion_downsample);
+          break;
+        default:
+          // change control type
+          if(!GET && value >= 0 && (int)value < 5) // if set command
+            motor->controller = (MotionControlType)value;
+          switch(motor->controller){
+            case MotionControlType::torque:
+              println(F("torque"));
+              break;
+            case MotionControlType::velocity:
+              println(F("vel"));
+              break;
+            case MotionControlType::angle:
+              println(F("angle"));
+              break;
+            case MotionControlType::velocity_openloop:
+              println(F("vel open"));
+              break;
+            case MotionControlType::angle_openloop:
+              println(F("angle open"));
+              break;
+          }
+            break;
+        }
+      break;
+    case CMD_TORQUE_TYPE:
+      // change control type
+      printVerbose(F("Torque: "));
+      if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
+        motor->torque_controller = (TorqueControlType)value;
+      switch(motor->torque_controller){
+        case TorqueControlType::voltage:
+          println(F("volt"));
+          // change the velocity control limits if necessary
+          if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
+          break;
+        case TorqueControlType::dc_current:
+          println(F("dc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+        case TorqueControlType::foc_current:
+          println(F("foc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+      }
+      break;
+    case CMD_STATUS:
+      // enable/disable
+      printVerbose(F("Status: "));
+      if(!GET) (bool)value ? motor->enable() : motor->disable();
+       println(motor->enabled);
+      break;
+    default:
+      target(motor,  user_cmd, separator);
+      break;
+  }
+}
+
+void Commander::pid(PIDController* pid, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_PID_P:      // P gain change
+      printVerbose("P: ");
+      if(!GET) pid->P = value;
+      println(pid->P);
+      break;
+    case SCMD_PID_I:      // I gain change
+      printVerbose("I: ");
+      if(!GET) pid->I = value;
+      println(pid->I);
+      break;
+    case SCMD_PID_D:      // D gain change
+      printVerbose("D: ");
+      if(!GET) pid->D = value;
+      println(pid->D);
+      break;
+    case SCMD_PID_RAMP:      //  ramp change
+      printVerbose("ramp: ");
+      if(!GET) pid->output_ramp = value;
+      println(pid->output_ramp);
+      break;
+    case SCMD_PID_LIM:      //  limit change
+      printVerbose("limit: ");
+      if(!GET) pid->limit = value;
+      println(pid->limit);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_LPF_TF:      // Tf value change
+      printVerbose(F("Tf: "));
+      if(!GET) lpf->Tf = value;
+      println(lpf->Tf);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::scalar(float* value,  char* user_cmd){
+  bool GET  = isSentinel(user_cmd[0]);
+  if(!GET) *value = atof(user_cmd);
+  println(*value);
+}
+
+
+void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
+  // if no values sent
+  if(isSentinel(user_cmd[0])) {
+    printlnMachineReadable(motor->target);
+    return;
+  };
+
+  float pos, vel, torque;
+  char* next_value;
+  switch(motor->controller){
+    case MotionControlType::torque: // setting torque target
+      torque = atof(strtok (user_cmd, separator));
+      motor->target = torque;
+      break;
+    case MotionControlType::velocity: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value){
+        torque = atof(next_value);
+        motor->PID_velocity.limit = torque;
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle: // setting angle target + torque, velocity limit
+      // setting the target position
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos;
+
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        motor->P_angle.limit = vel;
+  
+        // allow for setting only the target position and velocity limit without the torque limit 
+        next_value = strtok (NULL, separator);
+        if( next_value ){
+          torque= atof(next_value);
+          motor->PID_velocity.limit = torque;
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+    case MotionControlType::velocity_openloop: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value ){
+        torque = atof(next_value);
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
+      // set the target
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos; 
+      
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        // allow for setting only the target velocity without chaning the torque limit
+        next_value = strtok (NULL, separator);
+        if (next_value ){
+          torque = atof(next_value);
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+  }
+  printVerbose(F("Target: "));
+  println(motor->target);
+}
+
+
+bool Commander::isSentinel(char ch)
+{
+  if(ch == eol)
+    return true;
+  else if (ch == '\r')
+  {
+      printVerbose(F("Warn: \\r detected! \n"));
+      return true; // lets still consider it to end the line...
+  }
+  return false;
+}
+
+void Commander::print(const int number){
+  if( !com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(number);
+}
+void Commander::print(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print((float)number,(int)decimal_places);
+}
+void Commander::print(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+
+void Commander::println(const int number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(number);
+}
+void Commander::println(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println((float)number, (int)decimal_places);
+}
+void Commander::println(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+
+
+void Commander::printVerbose(const char* message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+void Commander::printVerbose(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+
+void Commander::printMachineReadable(const int number){
+  if(verbose == VerboseMode::machine_readable) print(number);
+}
+void Commander::printMachineReadable(const float number){
+  if(verbose == VerboseMode::machine_readable) print(number);
+}
+void Commander::printMachineReadable(const char* message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+void Commander::printMachineReadable(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+void Commander::printMachineReadable(const char message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+
+void Commander::printlnMachineReadable(const int number){
+  if(verbose == VerboseMode::machine_readable) println(number);
+}
+void Commander::printlnMachineReadable(const float number){
+  if(verbose == VerboseMode::machine_readable) println(number);
+}
+void Commander::printlnMachineReadable(const char* message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+void Commander::printlnMachineReadable(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+void Commander::printlnMachineReadable(const char message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+
+void Commander::printError(){
+ println(F("err"));
+}

firmware/lib/Arduino-FOC/src/communication/Commander.h

@@ -0,0 +1,301 @@
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#include "Arduino.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "../common/pid.h"
+#include "../common/lowpass_filter.h"
+#include "commands.h"
+
+
+#define MAX_COMMAND_LENGTH 20
+
+
+// Commander verbose display to the user type
+enum VerboseMode : uint8_t {
+  nothing       = 0x00, // display nothing - good for monitoring
+  on_request    = 0x01, // display only on user request
+  user_friendly = 0x02,  // display textual messages to the user
+  machine_readable = 0x03 // display machine readable commands, matching commands to set each settings
+};
+
+
+// callback function pointer definiton
+typedef void (* CommandCallback)(char*); //!< command callback function pointer
+
+/**
+ * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`)
+ *
+ *  - This class can be used in combination with HardwareSerial instance which it would read and write
+ *    or it can be used to parse strings that have been received from the user outside this library
+ *  - Commander class implements command protocol for few standard components of the SimpleFOC library
+ *     - FOCMotor
+ *     - PIDController
+ *     - LowPassFilter
+ *  - Commander also provides a very simple command > callback interface that enables user to
+ *    attach a callback function to certain command id - see function add()
+ */
+class Commander
+{
+  public:
+    /**
+     * Default constructor receiving a serial interface that it uses to output the values to
+     * Also if the function run() is used it uses this serial instance to read the serial for user commands
+     *
+     * @param serial - Serial com port instance
+     * @param eol - the end of line sentinel character
+     * @param echo - echo last typed character (for command line feedback)
+     */
+    Commander(Stream &serial, char eol = '\n', bool echo = false);
+    Commander(char eol = '\n', bool echo = false);
+
+    /**
+     * Function reading the serial port and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     */
+    void run();
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param reader - temporary stream to read user input
+     * @param eol - temporary end of line sentinel
+     */
+    void run(Stream &reader, char eol = '\n');
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param user_input - string of user inputs
+     */
+    void run(char* user_input);
+
+    /**
+     *  Function adding a callback to the coomander withe the command id
+     * @param id         - char command letter
+     * @param onCommand  - function pointer void function(char*)
+     * @param label      - string label to be displayed when scan command sent
+     */
+    void add(char id , CommandCallback onCommand, char* label = nullptr);
+
+    // printing variables
+    VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text
+    uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers
+
+    // monitoring functions
+    Stream* com_port = nullptr; //!< Serial terminal variable if provided
+    char eol = '\n'; //!< end of line sentinel character
+    bool echo = false; //!< echo last typed character (for command line feedback)
+
+    /**
+     *
+     * FOC motor (StepperMotor and BLDCMotor) command interface
+     * @param motor    - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *    'Q' - Q current PID controller & LPF (see function pid and lpf for commands)
+     *    'D' - D current PID controller & LPF (see function pid and lpf for commands)
+     *    'V' - Velocity PID controller & LPF  (see function pid and lpf for commands)
+     *    'A' - Angle PID controller & LPF     (see function pid and lpf for commands)
+     *    'L' - Limits
+     *           sub-commands:
+     *           'C' - Current
+     *           'U' - Voltage
+     *           'V' - Velocity
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    'R' - Motor resistance
+     *    'S' - Sensor offsets
+     *          sub-commands:
+     *          'M' - sensor offset
+     *          'E' - sensor electrical zero
+     *    'M' - Monitoring control
+     *          sub-commands:
+     *          'D' - downsample monitoring
+     *          'C' - clear monitor
+     *          'S' - set monitoring variables
+     *          'G' - get variable value
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     *
+     *  - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance)
+     *  - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f)
+     *
+     */
+    void motor(FOCMotor* motor, char* user_cmd);
+
+    /**
+     * Low pass fileter command interface
+     * @param lpf      - LowPassFilter instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - filtering time constant Tf
+     *  - It can be get by sending 'F'
+     *  - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01)
+     */
+    void lpf(LowPassFilter* lpf, char* user_cmd);
+    /**
+     * PID controller command interface
+     * @param pid      - PIDController instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *     - P gain       - 'P'
+     *     - I gain       - 'I'
+     *     - D gain       - 'D'
+     *     - output ramp  - 'R'
+     *     - output limit - 'L'
+     *  - Each of them can be get by sening the command letter -(ex. 'D' - to get the D gain)
+     *  - Each of them can be set by sending 'IDvalue' - (ex. I1.5 for setting I=1.5)
+     */
+    void pid(PIDController* pid, char* user_cmd);
+    /**
+     * Float variable scalar command interface
+     * @param value    - float variable pointer 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - one float value
+     *  - It can be get by sending an empty string '\n'
+     *  - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01)
+     */
+    void scalar(float* value, char* user_cmd);
+    /**
+     *  Target setting interface, enables setting the target and limiting variables at once. 
+     *  The values are sent separated by a separator specified as the third argument. The default separator is the space.
+     * 
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     *  
+     *  Example: P2.34 70 2
+     *  `P` is the user defined command, `2.34` is the target angle `70` is the target 
+     *  velocity and `2` is the desired max current.
+     *  
+     *  It depends of the motion control mode:
+     *    - torque   : torque (ex. P2.5) 
+     *    - velocity : velocity torque (ex.P10 2.5 or P10 to only chanage the target witout limits)
+     *    - angle    : angle velocity torque (ex.P3.5 10 2.5 or P3.5 to only chanage the target witout limits)
+     */
+    void target(FOCMotor* motor, char* user_cmd, char* separator = (char *)" ");
+
+    /**
+     * FOC motor (StepperMotor and BLDCMotor) motion control interfaces
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     * 
+     * Commands:
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     */
+    void motion(FOCMotor* motor, char* user_cmd, char* separator = (char *)" ");
+
+    bool isSentinel(char ch);
+  private:
+    // Subscribed command callback variables
+    CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number
+    char call_ids[20]; //!< added callback commands
+    char* call_label[20]; //!< added callback labels
+    int call_count = 0;//!< number callbacks that are subscribed
+
+    // helping variable for serial communication reading
+    char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline
+    int rec_cnt = 0; //!< number of characters receives
+
+    // serial printing functions
+    /**
+     *  print the string message only if verbose mode on
+     *  @param message - message to be printed
+     */
+    void printVerbose(const char* message);
+    /**
+     *  Print the string message only if verbose mode on
+     *  - Function handling the case for strings defined by F macro
+     *  @param message - message to be printed
+     */
+    void printVerbose(const __FlashStringHelper *message);
+    /**
+     *  print the numbers to the serial with desired decimal point number
+     *  @param message - number to be printed
+     *  @param newline - if needs lewline (1) otherwise (0)
+     */
+
+    void print(const float number);
+    void print(const int number);
+    void print(const char* message);
+    void print(const __FlashStringHelper *message);
+    void print(const char message);
+    void println(const float number);
+    void println(const int number);
+    void println(const char* message);
+    void println(const __FlashStringHelper *message);
+    void println(const char message);
+
+    void printMachineReadable(const float number);
+    void printMachineReadable(const int number);
+    void printMachineReadable(const char* message);
+    void printMachineReadable(const __FlashStringHelper *message);
+    void printMachineReadable(const char message);
+
+    void printlnMachineReadable(const float number);
+    void printlnMachineReadable(const int number);
+    void printlnMachineReadable(const char* message);
+    void printlnMachineReadable(const __FlashStringHelper *message);
+    void printlnMachineReadable(const char message);
+
+
+    void printError();
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/communication/SimpleFOCDebug.cpp

@@ -0,0 +1,104 @@
+
+#include "SimpleFOCDebug.h"
+
+#ifndef SIMPLEFOC_DISABLE_DEBUG
+
+
+Print* SimpleFOCDebug::_debugPrint = NULL;
+
+
+void SimpleFOCDebug::enable(Print* debugPrint) {
+    _debugPrint = debugPrint;
+}
+
+
+void SimpleFOCDebug::println(int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(val);
+    }
+}
+
+
+
+void SimpleFOCDebug::println(const char* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(str);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(str);
+    }
+}
+
+void SimpleFOCDebug::println(const char* str, float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const char* str, int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+
+void SimpleFOCDebug::print(const char* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+    }
+}
+
+
+void SimpleFOCDebug::print(const __FlashStringHelper* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+    }
+}
+
+
+void SimpleFOCDebug::print(int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(val);
+    }
+}
+
+
+void SimpleFOCDebug::print(float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(val);
+    }
+}
+
+
+void SimpleFOCDebug::println() {
+    if (_debugPrint != NULL) {
+        _debugPrint->println();
+    }
+}
+
+#endif

firmware/lib/Arduino-FOC/src/communication/SimpleFOCDebug.h

@@ -0,0 +1,79 @@
+
+#ifndef __SIMPLEFOCDEBUG_H__
+#define __SIMPLEFOCDEBUG_H__
+
+#include "Arduino.h"
+
+
+/**
+ * SimpleFOCDebug class
+ * 
+ * This class is used to print debug messages to a chosen output.
+ * Currently, Print instances are supported as targets, e.g. serial port.
+ * 
+ * Activate debug output globally by calling enable(), optionally passing
+ * in a Print instance. If none is provided "Serial" is used by default.
+ * 
+ * To produce debug output, use the macro SIMPLEFOC_DEBUG:
+ *   SIMPLEFOC_DEBUG("Debug message!");
+ *   SIMPLEFOC_DEBUG("a float value:", 123.456f);
+ *   SIMPLEFOC_DEBUG("an integer value: ", 123);
+ * 
+ * Keep debugging output short and simple. Some of our MCUs have limited
+ * RAM and limited serial output capabilities.
+ * 
+ * By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to
+ * help preserve memory on Arduino boards.
+ * 
+ * You can also disable debug output completely. In this case all debug output 
+ * and the SimpleFOCDebug class is removed from the compiled code.
+ * Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in
+ * this way.
+ * 
+ **/
+
+
+#ifndef SIMPLEFOC_DISABLE_DEBUG
+
+class SimpleFOCDebug {
+public:
+    static void enable(Print* debugPrint = &Serial);
+
+    static void println(const __FlashStringHelper* msg);
+    static void println(const char* msg);
+    static void println(const __FlashStringHelper* msg, float val);
+    static void println(const char* msg, float val);
+    static void println(const __FlashStringHelper* msg, int val);
+    static void println(const char* msg, int val);
+    static void println();
+    static void println(int val);
+    static void println(float val);
+
+    static void print(const char* msg);
+    static void print(const __FlashStringHelper* msg);
+    static void print(int val);
+    static void print(float val);
+
+protected:
+    static Print* _debugPrint;
+};
+
+
+#define SIMPLEFOC_DEBUG(msg, ...) \
+    SimpleFOCDebug::println(F(msg), ##__VA_ARGS__)
+
+
+
+
+
+#else //ifndef SIMPLEFOC_DISABLE_DEBUG
+
+
+
+#define SIMPLEFOC_DEBUG(msg, ...)
+
+
+
+#endif //ifndef SIMPLEFOC_DISABLE_DEBUG
+#endif
+

firmware/lib/Arduino-FOC/src/communication/StepDirListener.cpp

@@ -0,0 +1,40 @@
+#include "StepDirListener.h"
+
+StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _counter_to_value){
+    pin_step = _pinStep;
+    pin_dir = _pinDir;
+    counter_to_value = _counter_to_value;
+}
+
+void StepDirListener::init(){
+    pinMode(pin_dir, INPUT);
+    pinMode(pin_step, INPUT_PULLUP);
+    count = 0;
+}
+
+void StepDirListener::enableInterrupt(void (*doA)()){
+    attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity);
+}
+
+void StepDirListener::attach(float* variable){
+    attached_variable = variable;
+}
+
+void StepDirListener::handle(){ 
+  // read step status
+  //bool step = digitalRead(pin_step);
+  // update counter only on rising edge 
+  //if(step && step != step_active){
+    if(digitalRead(pin_dir)) 
+        count++;
+    else 
+        count--;
+   //}
+   //step_active = step;
+   // if attached variable update it
+   if(attached_variable) *attached_variable = getValue();
+}
+// calculate the position from counter
+float StepDirListener::getValue(){
+    return (float) count * counter_to_value;
+}

firmware/lib/Arduino-FOC/src/communication/StepDirListener.h

@@ -0,0 +1,65 @@
+#ifndef STEPDIR_H
+#define STEPDIR_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+
+
+#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR)
+#define PinStatus int
+#endif
+
+
+/**
+ * Step/Dir listenner class for easier interraction with this communication interface.
+ */
+class StepDirListener
+{
+  public:
+
+    /**
+     * Constructor for step/direction interface
+     *  @param step              - pin
+     *  @param direction         - pin
+     *  @param counter_to_value  - step counter to value
+     */
+    StepDirListener(int pinStep, int pinDir, float counter_to_value = 1);
+    /**
+     * Start listenning for step commands
+     * 
+     * @param handleStep - on step received handler
+     */
+    void enableInterrupt(void (*handleStep)());
+
+    /**
+     * Initialise dir and step commands
+     */
+    void init();
+    /**
+     * step handler
+     */
+    void handle();
+    /**
+     * Get so far received valued
+     */
+    float getValue();
+    /**
+     * Attach the value to be updated on each step receive 
+     * - no need to call getValue function
+     */
+    void attach(float* variable);
+
+    // variables
+    int pin_step; //!< step pin
+    int pin_dir; //!< direction pin
+    long count; //!< current counter value - should be set to 0 for homing
+    PinStatus polarity = RISING; //!< polarity of the step pin
+
+  private:
+    float* attached_variable = nullptr; //!< pointer to the attached variable 
+    float counter_to_value; //!< step counter to value 
+    //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable
+
+};
+
+#endif

firmware/lib/Arduino-FOC/src/communication/commands.h

@@ -0,0 +1,52 @@
+#ifndef COMMANDS_h
+#define  COMMANDS_h
+
+// see docs.simplefoc.com for in depth explanation of the commands
+
+// list of commands
+ #define CMD_C_D_PID   'D' //!< current d PID & LPF
+ #define CMD_C_Q_PID   'Q' //!< current d PID & LPF
+ #define CMD_V_PID     'V' //!< velocity PID & LPF
+ #define CMD_A_PID     'A' //!< angle PID & LPF
+ #define CMD_STATUS    'E' //!< motor status enable/disable
+ #define CMD_LIMITS    'L' //!< limits current/voltage/velocity
+ #define CMD_MOTION_TYPE  'C' //!< motion control type
+ #define CMD_TORQUE_TYPE  'T' //!< torque control type
+ #define CMD_SENSOR    'S' //!< sensor offsets
+ #define CMD_MONITOR   'M' //!< monitoring
+ #define CMD_RESIST    'R' //!< motor phase resistance
+ #define CMD_INDUCTANCE    'I' //!< motor phase inductance
+ #define CMD_KV_RATING 'K' //!< motor kv rating
+ #define CMD_PWMMOD   'W' //!< pwm modulation
+
+ // commander configuration
+ #define CMD_SCAN    '?' //!< command scaning the network - only for commander
+ #define CMD_VERBOSE '@' //!< command setting output mode - only for commander
+ #define CMD_DECIMAL '#' //!< command setting decimal places - only for commander
+
+ // subcomands
+ //pid - lpf
+ #define SCMD_PID_P     'P' //!< PID gain P
+ #define SCMD_PID_I     'I' //!< PID gain I
+ #define SCMD_PID_D     'D' //!< PID gain D
+ #define SCMD_PID_RAMP  'R' //!< PID ramp
+ #define SCMD_PID_LIM   'L' //!< PID limit
+ #define SCMD_LPF_TF    'F' //!< LPF time constant
+ // limits
+ #define SCMD_LIM_CURR  'C' //!< Limit current
+ #define SCMD_LIM_VOLT  'U' //!< Limit voltage
+ #define SCMD_LIM_VEL   'V' //!< Limit velocity
+ //sensor
+ #define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset
+ #define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset
+ // monitoring
+ #define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value
+ #define SCMD_CLEAR      'C' //!< Clear all monitored variables
+ #define SCMD_GET        'G' //!< Get variable only one value
+ #define SCMD_SET        'S' //!< Set variables to be monitored
+
+ #define SCMD_PWMMOD_TYPE   'T'  //!<< Pwm modulation type
+ #define SCMD_PWMMOD_CENTER 'C'  //!<< Pwm modulation center flag
+
+
+#endif