Merge remote-tracking branch 'simplefoc/dev' into dev

runger1101001 · runger1101001 · commit 3cc69336623d · 2021-04-29T23:46:26.000+02:00
diff --git a/README.md b/README.md
@@ -20,6 +20,9 @@ Therefore this is an attempt to:
 > - bugfixes commander
 > - bugfix `voltage_limit` when provided `phase_resistance`
 > - bugfix `hall_sensor` examples 
+> - added examples fot the powershield
+> - added initial support for `MagneticSensorPWM`
+> - extension of the `Commander` interface
 
 
 ## Arduino *SimpleFOCShield* v2.0.3
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/find_raw_min_max/find_raw_min_max.ino
@@ -0,0 +1,30 @@
+#include <SimpleFOC.h>
+
+
+/**
+ * An example to find out the raw max and min count to be provided to the constructor
+ * SPin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value 
+ * And replace values 4 and 904 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle(). 
+ * If there is a jump that means you can still find better values. 
+ */
+MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
+void doPWM(){sensor.handlePWM();}
+
+void setup() {
+  // monitoring port
+  Serial.begin(115200);
+
+  // initialise magnetic sensor hardware
+  sensor.init();
+  sensor.enableInterrupt(doPWM);
+
+  Serial.println("Sensor ready");
+  _delay(1000);
+}
+
+void loop() {
+  // display the angle and the angular velocity to the terminal
+  Serial.print(sensor.pulse_length_us);
+  Serial.print("\t");
+  Serial.println(sensor.getAngle());
+}
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm/magnetic_sensor_pwm.ino
@@ -0,0 +1,32 @@
+#include <SimpleFOC.h>
+
+
+/**
+ * Magnetic sensor reading pwm signal on pin 2.  The pwm duty cycle is proportional to the sensor angle.
+ * 
+ * MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
+ * - pinPWM         - the pin that is reading the pwm from magnetic sensor
+ * - min_raw_count  - the smallest expected reading.  Whilst you might expect it to be 0 it is often ~5.  Getting this wrong results in a small click once per revolution
+ * - max_raw_count  - the largest value read.  whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
+ */
+MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
+void doPWM(){sensor.handlePWM();}
+
+void setup() {
+  // monitoring port
+  Serial.begin(115200);
+
+  // initialise magnetic sensor hardware
+  sensor.init();
+  sensor.enableInterrupt(doPWM);
+
+  Serial.println("Sensor ready");
+  _delay(1000);
+}
+
+void loop() {
+  // display the angle and the angular velocity to the terminal
+  Serial.print(sensor.getAngle());
+  Serial.print("\t");
+  Serial.println(sensor.getVelocity());
+}
diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_pwm_example/magnetic_sensor_pwm_software_interrupt/magnetic_sensor_pwm_software_interrupt.ino
@@ -0,0 +1,38 @@
+#include <SimpleFOC.h>
+
+// software interrupt library
+#include <PciManager.h>
+#include <PciListenerImp.h>
+
+/**
+ * Magnetic sensor reading analog voltage on pin which does not have hardware interrupt support. Such as A0.
+ * 
+ * MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
+ * - pinPWM         - the pin that is reading the pwm from magnetic sensor
+ * - min_raw_count  - the smallest expected reading.  Whilst you might expect it to be 0 it is often ~5.  Getting this wrong results in a small click once per revolution
+ * - max_raw_count  - the largest value read.  whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
+ */
+MagneticSensorPWM sensor = MagneticSensorPWM(A0, 4, 904);
+void doPWM(){sensor.handlePWM();}
+
+// encoder interrupt init
+PciListenerImp listenerPWM(sensor.pinPWM, doPWM);
+
+void setup() {
+  // monitoring port
+  Serial.begin(115200);
+
+  // initialise magnetic sensor hardware
+  sensor.init();
+  PciManager.registerListener(&listenerPWM);
+
+  Serial.println("Sensor ready");
+  _delay(1000);
+}
+
+void loop() {
+  // display the angle and the angular velocity to the terminal
+  Serial.print(sensor.getAngle());
+  Serial.print("\t");
+  Serial.println(sensor.getVelocity());
+}
diff --git a/keywords.txt b/keywords.txt
@@ -8,6 +8,7 @@ HallSensors	KEYWORD1
 MagneticSensorSPI	KEYWORD1
 MagneticSensorI2C	KEYWORD1
 MagneticSensorAnalog	KEYWORD1
+MagneticSensorPWM	KEYWORD1
 BLDCDriver3PWM	KEYWORD1    
 BLDCDriver6PWM	KEYWORD1    
 BLDCDriver	KEYWORD1    
@@ -92,6 +93,8 @@ scalar	KEYWORD2
 pid	KEYWORD2
 lpf	KEYWORD2
 motor	KEYWORD2
+handlePWM	KEYWORD2
+enableInterrupt	KEYWORD2
 
 
 
@@ -119,6 +122,8 @@ sensor_offset	KEYWORD2
 zero_electric_angle	KEYWORD2
 verbose	KEYWORD2
 decimal_places	KEYWORD2
+phase_resistance	KEYWORD2
+modulation_centered	KEYWORD2
 
 
 
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
@@ -17,8 +17,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
         i_alpha = current.a;  
         i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
     }else{
-        i_alpha = 2*(current.a - (current.b - current.c))/3.0;    
-        i_beta = _2_SQRT3 *( current.b  - current.c );
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f/3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
     }
 
     // if motor angle provided function returns signed value of the current
@@ -44,9 +48,13 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
         // if only two measured currents
         i_alpha = current.a;  
         i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }else{
-        i_alpha = 0.6666667*(current.a - (current.b - current.c));    
-        i_beta = _2_SQRT3 *( current.b  - current.c );
+    } else {
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f/3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
     }
 
     // calculate park transform
diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h
@@ -169,7 +169,7 @@ class FOCMotor
     LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
     LowPassFilter LPF_current_d{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
     PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration
-    PIDController P_angle{DEF_P_ANGLE_P,0,0,1e10,DEF_VEL_LIM};	//!< parameter determining the position PID configuration 
+    PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM};	//!< parameter determining the position PID configuration 
     LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!<  parameter determining the velocity Low pass filter configuration 
     LowPassFilter LPF_angle{0.0};//!<  parameter determining the angle low pass filter configuration 
     unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
diff --git a/src/common/defaults.h b/src/common/defaults.h
@@ -23,7 +23,7 @@
 #define DEF_PID_CURR_P 3 //!< default PID controller P value
 #define DEF_PID_CURR_I 300.0 //!<  default PID controller I value
 #define DEF_PID_CURR_D 0.0 //!<  default PID controller D value
-#define DEF_PID_CURR_RAMP 1e11 //!< default PID controller voltage ramp value
+#define DEF_PID_CURR_RAMP 0  //!< default PID controller voltage ramp value
 #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
 #define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant
 #endif
diff --git a/src/common/pid.cpp b/src/common/pid.cpp
@@ -40,13 +40,15 @@ float PIDController::operator() (float error){
     // antiwindup - limit the output variable
     output = _constrain(output, -limit, limit);
 
-    // limit the acceleration by ramping the output
-    float output_rate = (output - output_prev)/Ts;
-    if (output_rate > output_ramp)
-        output = output_prev + output_ramp*Ts;
-    else if (output_rate < -output_ramp)
-        output = output_prev - output_ramp*Ts;
-        
+    // if output ramp defined
+    if(output_ramp > 0){
+        // limit the acceleration by ramping the output
+        float output_rate = (output - output_prev)/Ts;
+        if (output_rate > output_ramp)
+            output = output_prev + output_ramp*Ts;
+        else if (output_rate < -output_ramp)
+            output = output_prev - output_ramp*Ts;
+    }
     // saving for the next pass
     integral_prev = integral;
     output_prev = output;
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
@@ -170,10 +170,10 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
       }
       break;
     case CMD_MOTION_TYPE:
-      printVerbose(F("Motion: "));
+      printVerbose(F("Motion:"));
       switch(sub_cmd){
         case SCMD_DOWNSAMPLE:
-            printVerbose(F("downsample: "));
+            printVerbose(F(" downsample: "));
             if(!GET) motor->motion_downsample = value;
             println((int)motor->motion_downsample);
           break;
@@ -224,6 +224,38 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
       if(!GET) (bool)value ? motor->enable() : motor->disable();
        println(motor->enabled);
       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:
       // enable/disable
       printVerbose(F("R phase: "));
@@ -271,15 +303,15 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
               break;
             case 2: // get voltage d
               printVerbose(F("Vd: "));
-              println(motor->voltage.q);
+              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.q);
+              println(motor->current.d);
               break;
             case 5: // get velocity
               printVerbose(F("vel: "));
@@ -289,6 +321,22 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
               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;
diff --git a/src/communication/commands.h b/src/communication/commands.h
@@ -15,6 +15,7 @@
  #define CMD_SENSOR    'S' //!< sensor offsets
  #define CMD_MONITOR   'M' //!< monitoring
  #define CMD_RESIST    'R' //!< motor phase resistance
+ #define CMD_PWMMOD   'W' //!< pwm modulation
 
  // commander configuration
  #define CMD_SCAN    '?' //!< command scaning the network - only for commander
@@ -41,5 +42,9 @@
  #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
diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp
@@ -28,21 +28,23 @@ void InlineCurrentSense::init(){
 }
 // Function finding zero offsets of the ADC
 void InlineCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
     // find adc offset = zero current voltage
-    offset_ia =0;
-    offset_ib= 0;
-    offset_ic= 0;
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
     // read the adc voltage 1000 times ( arbitrary number )
-    for (int i = 0; i < 1000; i++) {
+    for (int i = 0; i < calibration_rounds; i++) {
         offset_ia += _readADCVoltage(pinA);
         offset_ib += _readADCVoltage(pinB);
         if(_isset(pinC)) offset_ic += _readADCVoltage(pinC);
         _delay(1);
     }
     // calculate the mean offsets
-    offset_ia = offset_ia / 1000.0;
-    offset_ib = offset_ib / 1000.0;
-    if(_isset(pinC)) offset_ic = offset_ic / 500.0;
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
 }
 
 // read all three phase currents (if possible 2 or 3)
diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h
@@ -29,9 +29,9 @@ class InlineCurrentSense: public CurrentSense{
     // ADC measuremnet gain for each phase
     // support for different gains for different phases of more commonly - inverted phase currents
     // this should be automated later
-  	int gain_a; //!< phase A gain 
-  	int gain_b; //!< phase B gain 
-  	int gain_c; //!< phase C gain 
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
 
   private:
 
diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp
diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h
