From 4845c5910aab8ec27649d13bb801ece7e01dfb4f Mon Sep 17 00:00:00 2001
From: Kevin J Walters <kjw.github@gmail.com>
Date: Fri, 12 Jan 2024 18:28:48 +0000
Subject: [PATCH] Improving resolution of servo control. Changing getEnvelop to
 return a float and using a locally defined mapf to avoid the coarse
 conversion which happens with int values of 2 to 10. #38

---
 software/ClawController/ClawController.ino | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/software/ClawController/ClawController.ino b/software/ClawController/ClawController.ino
index 361f033..a158fcf 100644
--- a/software/ClawController/ClawController.ino
+++ b/software/ClawController/ClawController.ino
@@ -26,6 +26,7 @@
 // SOFTWARE.
 
 #include <inttypes.h>
+#include <math.h>
 
 #if defined(ESP32) 
   #include <ESP32Servo.h>
@@ -49,6 +50,10 @@ int data_index;
 int flag=0;
 Servo servo;
 
+inline float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 void setup() {
   // Serial connection begin
   Serial.begin(BAUD_RATE);
@@ -84,8 +89,10 @@ void loop() {
     timer += 1000000 / SAMPLE_RATE;
     int sensor_value = analogRead(INPUT_PIN);
     int signal = (int)EMGFilter((float)sensor_value);
-    int envelop = getEnvelop(abs(signal));
-    int servo_position = constrain(map(envelop, EMG_MIN, EMG_MAX, SERVO_MIN, SERVO_MAX),
+    float envelop = getEnvelop(abs(signal));
+    int servo_position = constrain((int)roundf(mapf(envelop,
+                                                    EMG_MIN, EMG_MAX,
+                                                    SERVO_MIN, SERVO_MAX)),
                                    SERVO_MIN, SERVO_MAX);
     servo.write(servo_position);
     Serial.print(signal);
@@ -95,12 +102,12 @@ void loop() {
 }
 
 // Envelop detection algorithm
-int getEnvelop(int abs_emg){
+float getEnvelop(int abs_emg){
   sum -= circular_buffer[data_index];
   sum += abs_emg;
   circular_buffer[data_index] = abs_emg;
   data_index = (data_index + 1) % BUFFER_SIZE;
-  return (sum/BUFFER_SIZE) * 2;
+  return (float(sum) / BUFFER_SIZE) * 2.0f;
 }
 
 // Band-Pass Butterworth IIR digital filter, generated using filter_gen.py.