Merge pull request #53 from dekutree64/dev

Improvements to SmoothingSensor and LinearHall
simplefoc · Jan 10, 2025 · 6d96569 · 6d96569
2 parents 5ed156f + 8818ad9
commit 6d96569
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 6 deletions.
diff --git a/examples/encoders/smoothing/smoothing.ino b/examples/encoders/smoothing/smoothing.ino
@@ -1,6 +1,9 @@
 /**
  *
- * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor
+ * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor. 
+ * The only changes are the declaration of the SmoothingSensor, passing it to motor.linkSensor 
+ * instead of the HallSensor instance, and the added Commander code to switch between the two.
+ *
  * Steps:
  * 1) Configure the motor and sensor
  * 2) Run the code
@@ -63,8 +66,6 @@ void setup() {
   sensor.enableInterrupts(doA, doB); //, doC);
   // software interrupts
   PciManager.registerListener(&listenerIndex);
-  // set SmoothingSensor phase correction for hall sensors
-  smooth.phase_correction = -_PI_6;
   // link the SmoothingSensor to the motor
   motor.linkSensor(&smooth);
 

diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp
@@ -13,6 +13,8 @@ LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
   pinA = _hallA;
   pinB = _hallB;
   pp = _pp;
+  electrical_rev = 0;
+  prev_reading = 0;
 }
 
 float LinearHall::getSensorAngle() {
@@ -84,7 +86,7 @@ void LinearHall::init(FOCMotor *motor) {
   // move one mechanical revolution forward
   for (int i = 0; i <= 2000; i++)
   {
-    float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
+    float angle = _3PI_2 + _2PI * i * motor->pole_pairs / pp / 2000.0f;
     motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);
 
     ReadLinearHalls(pinA, pinB, &lastA, &lastB);

diff --git a/src/encoders/smoothing/SmoothingSensor.cpp b/src/encoders/smoothing/SmoothingSensor.cpp
@@ -1,12 +1,16 @@
 #include "SmoothingSensor.h"
 #include "common/foc_utils.h"
 #include "common/time_utils.h"
+#include "sensors/HallSensor.h"
 
 
 SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m)
 {
 }
 
+SmoothingSensor::SmoothingSensor(HallSensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) {
+  phase_correction = -_PI_6;
+}
 
 void SmoothingSensor::update() {
   // Update sensor, with optional downsampling of update rate
@@ -27,8 +31,8 @@ void SmoothingSensor::update() {
 
   // Apply phase correction if needed
   if (phase_correction != 0) {
-    if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
-    else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+    if (_motor.shaft_velocity < -0.001) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+    else if (_motor.shaft_velocity > 0.001) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
   }
 
   // Handle wraparound of the projected angle

diff --git a/src/encoders/smoothing/SmoothingSensor.h b/src/encoders/smoothing/SmoothingSensor.h
@@ -22,6 +22,7 @@ class SmoothingSensor : public Sensor
     @param m  Motor that the SmoothingSensor will be linked to
     */
     SmoothingSensor(Sensor& s, const FOCMotor& m);
+    SmoothingSensor(class HallSensor& s, const FOCMotor& m); // Automatically sets phase_correction
 
     void update() override;
     float getVelocity() override;