Different encoder sensor algorithm (#500)

* Fix PWMSensor reorder warning

* Change both ring buffer and encoder sensor implementation a bit

* Remove execution period check

* Add direction and make position, speed and acceleration output parameters on the constructor

* Small fixes to properly calculate speed

* Remove encoder registration check from get_counter method

Author: jmaralo

Inc/C++Utilities/RingBuffer.hpp

@@ -2,79 +2,64 @@
 
 #include "CppImports.hpp"
 
+template <typename T, size_t N>
+class RingBuffer {
+    std::array<T, N> buffer{};
 
-/**
- * @brief a Buffer class that acts as a RingBuffer using FIFO behaviour
- */
-template <typename item_type, size_t N>
-class RingBuffer{
-public:
-	RingBuffer(): msize(0), mcapacity(N), mhead_index(0), mlast_index(N){}
-
-
-	/**
-	 * @brief introduces a new item on the Ring Buffer unless it is full. Returns false when the Ring Buffer is full.
-	 */
-	bool push(item_type item){
-		if(msize == mcapacity){
-			return false;
-		}
-
-		msize++;
-		mlast_index++;
-		if(mlast_index >= mcapacity){
-			mlast_index = 0;
-		}
-
-		buffer[mlast_index] = item;
-		return true;
-	}
-
-
-	bool is_empty(){return msize == 0;}  /**< @brief returns true when RingBuffer is empty, false otherwise.*/
-	bool is_full(){return msize == mcapacity;}  /**< @brief returns true when RingBuffer is full, false otherwise.*/
-	uint32_t size(){return msize;}	/**< @brief returns the occupied size of the RingBuffer.*/
-
-
-
-
-	/**
-	 * @brief checks the next variable to read without removing it from the buffer. Do not use on empty buffer.
-	 */
-	item_type peek(){
-		return buffer[mhead_index];
-	}
-
-	/**
-	 * @brief checks the next variable to read and removes it from the buffer. Do not use on empty buffer.
-	 */
-	item_type pop(){
-		uint32_t return_index = mhead_index;
-		mhead_index++;
-		msize--;
-		if(mhead_index >= mcapacity){
-			mhead_index = 0;
-		}
-		return buffer[return_index];
-	}
-
-	/**
-	 * @brief pop later and push new. Don't care size of buffer, needs to be initialize before
-	 */
-	void push_pop(item_type item){
-		mhead_index = (mhead_index+1)%mcapacity;
-		mlast_index = (mlast_index+1)%mcapacity;
-		buffer[mhead_index] = item;
-	}
-
-	item_type latest()const{
-		return buffer[mlast_index];
-	}
-
-private:
-	std::array<item_type, N> buffer;
-	uint32_t msize;
-	uint32_t mcapacity;
-	uint32_t mhead_index;
-	uint32_t mlast_index;
+    size_t stored_items{0};
+
+    size_t front{0};
+    size_t back{0};
+
+    size_t move_forward(size_t origin, size_t amount) {
+        return (origin + amount) % N;
+    }
+
+    size_t move_backward(size_t origin, size_t amount) {
+        // N * ((amount / N) + 1) makes it so that the operation doesn't
+        // overflow (size_t is unsigned) and with the help of modular
+        // arithmetic, this won't change the result
+        return ((origin + (N * ((amount / N) + 1))) - amount) % N;
+    }
+
+   public:
+    RingBuffer() {}
+
+    bool push(T item) {
+        if (is_full()) return false;
+
+        buffer[front] = item;
+        front = move_forward(front, 1);
+        ++stored_items;
+
+        return true;
+    }
+
+    bool pop() {
+        if (is_empty()) return false;
+
+        back = move_forward(back, 1);
+        --stored_items;
+
+        return true;
+    }
+
+    bool push_pop(T item) {
+        if (is_empty()) return false;
+
+        buffer[front] = item;
+        front = move_forward(front, 1);
+        back = move_forward(back, 1);
+
+        return true;
+    }
+
+    T &operator[](size_t index) {
+        return buffer[move_backward(front, index + 1)];
+    }
+
+    constexpr size_t capacity() { return N; }
+    size_t size() { return stored_items; }
+    bool is_full() { return size() >= capacity(); }
+    bool is_empty() { return size() == 0; }
 };

Inc/ST-LIB_LOW/Sensors/EncoderSensor/EncoderSensor.hpp

@@ -7,40 +7,80 @@
  */
 
 #pragma once
-#include "HALAL/HALAL.hpp"
 #include "ErrorHandler/ErrorHandler.hpp"
+#include "HALAL/HALAL.hpp"
+
+template <size_t SAMPLES>
+class EncoderSensor {
+   public:
+    enum Direction : uint8_t { FORWARD = 0, BACKWARDS = 1 };
+
+   private:
+    constexpr static int64_t START_COUNTER{UINT32_MAX / 2};
+
+    const double counter_distance_m;
+    const double sample_time_s;
+
+    uint8_t encoder_id;
+
+    // We want to get the last buffer element and the midpoint, if the number of
+    // elements is odd, these points and the present won't be evenly spaced
+    // across time. The SAMPLES computation rounds it down to make it even
+    RingBuffer<int64_t, (SAMPLES / 2) * 2> past_delta_counters{};
+
+    Direction *direction;
+    double *position;
+    double *speed;
+    double *acceleration;
+
+   public:
+    EncoderSensor(Pin &pin1, Pin &pin2, const double counter_distance_m,
+                  const double sample_time_s, Direction *direction,
+                  double *position, double *speed, double *acceleration)
+        : counter_distance_m(counter_distance_m),
+          sample_time_s(sample_time_s),
+          encoder_id(Encoder::inscribe(pin1, pin2)),
+          direction(direction),
+          position(position),
+          speed(speed),
+          acceleration(acceleration) {
+        for (size_t i{0}; i < SAMPLES; ++i) past_delta_counters.push(0);
+    }
+
+    void turn_on() { Encoder::turn_on(encoder_id); }
+    void turn_off() { Encoder::turn_off(encoder_id); }
+
+    void reset() {
+        Encoder::reset(encoder_id);
+        for (size_t i{0}; i < SAMPLES; ++i) past_delta_counters.push_pop(0);
+    }
+
+    // must be called on equally spaced time periods
+    void read() {
+        uint32_t counter{Encoder::get_counter(encoder_id)};
+
+        int64_t delta_counter{(int64_t)counter - START_COUNTER};
+        const int64_t &previous_delta_counter{
+            past_delta_counters[past_delta_counters.size() / 2 - 1]};
+        const int64_t &previous_previous_delta_counter{
+            past_delta_counters[past_delta_counters.size() - 1]};
+
+        *position = delta_counter * counter_distance_m;
+
+        // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Backward_finite_difference
+        *speed = ((3.0 * delta_counter / 2.0) - (2.0 * previous_delta_counter) +
+                  (previous_previous_delta_counter / 2.0)) *
+                 counter_distance_m /
+                 (sample_time_s * past_delta_counters.size() / 2);
+
+        *acceleration = (delta_counter - (2.0 * previous_delta_counter) +
+                         previous_previous_delta_counter) *
+                        counter_distance_m /
+                        ((sample_time_s * past_delta_counters.size() / 2) *
+                         (sample_time_s * past_delta_counters.size() / 2));
+
+        *direction = Encoder::get_direction(encoder_id) ? FORWARD : BACKWARDS;
 
-#define COUNTER_DISTANCE_IN_METERS 0.0001
-#define N_FRAMES 100
-#define FRAME_SIZE_IN_SECONDS 0.005
-#define START_COUNTER UINT32_MAX / 2
-
-
-
-class EncoderSensor{
-public:
-	EncoderSensor() = default;
-	EncoderSensor(Pin pin1, Pin pin2, double* position, bool* direction, double* speed, double* acceleration);
-	void start();
-	void read();
-	void reset();
-	uint8_t get_id();
-
-protected:
-	uint8_t id;
-	double* position;
-	bool* direction;
-	double* speed;
-	double* acceleration;
-	double time;
-	// double positions[N_FRAMES];
-	// double times[N_FRAMES];
-	// double speeds[N_FRAMES];
-	RingBuffer<double, N_FRAMES> positions;
-	RingBuffer<double, N_FRAMES> times;
-	RingBuffer<double, N_FRAMES> speeds;
-	uint64_t last_clock_time;
-
-private:
-	void update_arrays();
+        past_delta_counters.push_pop(delta_counter);
+    }
 };

Inc/ST-LIB_LOW/Sensors/PWMSensor/PWMSensor.hpp

@@ -27,7 +27,7 @@ class PWMSensor {
 
 template<class Type>
 PWMSensor<Type>::PWMSensor(Pin &pin, Type &frequency, Type &duty_cycle) :
-	frequency(&frequency), duty_cycle(&duty_cycle) {
+	duty_cycle(&duty_cycle), frequency(&frequency)  {
 		id = InputCapture::inscribe(pin);
 		Sensor::inputcapture_id_list.push_back(id);
 }

Src/HALAL/Services/Encoder/Encoder.cpp

@@ -86,11 +86,6 @@ void Encoder::reset(uint8_t id) {
 }
 
 uint32_t Encoder::get_counter(uint8_t id) {
-    // if (not Encoder::registered_encoder.contains(id)) {
-    // 	ErrorHandler("No encoder registered with id %u", id);
-    // 	return 0;
-    // }
-
     TimerPeripheral* timer = pin_timer_map[registered_encoder[id]];
 
     return timer->handle->Instance->CNT;
@@ -146,12 +141,16 @@ void Encoder::init(TimerPeripheral* encoder) {
     }
 }
 
-int64_t Encoder::get_delta_clock(uint64_t clock_time, uint64_t last_clock_time){
-		int64_t delta_clock = clock_time - last_clock_time;
-		if(clock_time < last_clock_time){ //overflow handle
-			delta_clock = clock_time + CLOCK_MAX_VALUE * NANO_SECOND / HAL_RCC_GetPCLK1Freq()*2 - last_clock_time;
-		}
-		return delta_clock;
-	}
+int64_t Encoder::get_delta_clock(uint64_t clock_time,
+                                 uint64_t last_clock_time) {
+    int64_t delta_clock = clock_time - last_clock_time;
+    if (clock_time < last_clock_time) {  // overflow handle
+        delta_clock =
+            clock_time +
+            CLOCK_MAX_VALUE * NANO_SECOND / HAL_RCC_GetPCLK1Freq() * 2 -
+            last_clock_time;
+    }
+    return delta_clock;
+}
 
 #endif