try to fix submodule

firmware/lib/Arduino-FOC/src/common/base_classes/FOCMotor.cpp

@@ -0,0 +1,155 @@
+#include "FOCMotor.h"
+#include "../../communication/SimpleFOCDebug.h"
+
+/**
+ * Default constructor - setting all variabels to default values
+ */
+FOCMotor::FOCMotor()
+{
+  // maximum angular velocity to be used for positioning 
+  velocity_limit = DEF_VEL_LIM;
+  // maximum voltage to be set to the motor
+  voltage_limit = DEF_POWER_SUPPLY;
+  // not set on the begining
+  current_limit = DEF_CURRENT_LIM;
+
+  // index search velocity
+  velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
+  // sensor and motor align voltage
+  voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
+
+  // default modulation is SinePWM
+  foc_modulation = FOCModulationType::SinePWM;
+
+  // default target value
+  target = 0;
+  voltage.d = 0;
+  voltage.q = 0;
+  // current target values
+  current_sp = 0;
+  current.q = 0;
+  current.d = 0;
+
+  // voltage bemf 
+  voltage_bemf = 0;
+  
+  //monitor_port 
+  monitor_port = nullptr;
+  //sensor 
+  sensor_offset = 0.0f;
+  sensor = nullptr;
+  //current sensor 
+  current_sense = nullptr;
+}
+
+
+/**
+	Sensor linking method
+*/
+void FOCMotor::linkSensor(Sensor* _sensor) {
+  sensor = _sensor;
+}
+
+/**
+	CurrentSense linking method
+*/
+void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
+  current_sense = _current_sense;
+}
+
+// shaft angle calculation
+float FOCMotor::shaftAngle() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_angle;
+  return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
+}
+// shaft velocity calculation
+float FOCMotor::shaftVelocity() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_velocity;
+  return sensor_direction*LPF_velocity(sensor->getVelocity());
+}
+
+float FOCMotor::electricalAngle(){
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return electrical_angle;
+  return  _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()  - zero_electric_angle );
+}
+
+/**
+ *  Monitoring functions
+ */
+// function implementing the monitor_port setter
+void FOCMotor::useMonitoring(Print &print){
+  monitor_port = &print; //operate on the address of print
+  #ifndef SIMPLEFOC_DISABLE_DEBUG
+  SimpleFOCDebug::enable(&print);
+  SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
+  #endif
+}
+
+// utility function intended to be used with serial plotter to monitor motor variables
+// significantly slowing the execution down!!!!
+void FOCMotor::monitor() {
+  if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return;
+  monitor_cnt = 0;
+  if(!monitor_port) return;
+  bool printed = 0;
+
+  if(monitor_variables & _MON_TARGET){
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    monitor_port->print(target,monitor_decimals);    
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_Q) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.q,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_D) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.d,monitor_decimals);
+    printed= true;
+  }
+  // read currents if possible - even in voltage mode (if current_sense available)
+  if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
+    DQCurrent_s c = current;
+    if( current_sense && torque_controller != TorqueControlType::foc_current ){
+      c = current_sense->getFOCCurrents(electrical_angle);
+      c.q = LPF_current_q(c.q);
+      c.d = LPF_current_d(c.d);
+    }
+    if(monitor_variables & _MON_CURR_Q) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.q*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+    if(monitor_variables & _MON_CURR_D) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.d*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+  }
+ 
+  if(monitor_variables & _MON_VEL) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_velocity,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_ANGLE) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_angle,monitor_decimals);
+    printed= true;
+  }
+  if(printed){
+    if(monitor_end_char) monitor_port->println(monitor_end_char);
+    else monitor_port->println("");
+  }
+}   
+