Feat/server

.env_example

@@ -0,0 +1 @@
+ESP32_MAC_ADRESS=

README.md

@@ -26,6 +26,26 @@ Sensora offers a new approach: transforming visual information into tactile and
 
 [To be completed]
 
+## 🚀 Getting Started
+
+### Servo Controllers
+
+The project supports two servo control implementations:
+
+#### Option 1: I2C + PCA9685 (Local Control)
+```bash
+python3 src/Raspberry\ Pi/servo_controller.py
+```
+
+#### Option 2: BLE + ESP32 (Wireless Control)
+```bash
+python3 src/Raspberry\ Pi/ble_servo_controller.py
+```
+
+**Requirements for BLE:**
+- Ensure `ESP32_MAC_ADDRESS` is set in your `.env` file
+- Install dependencies: `pip install bleak python-dotenv numpy`
+
 ## Get Involved
 
 You're invited to join this project! Check out the [contributing guide](./CONTRIBUTING.md).

requirements.txt

@@ -0,0 +1,3 @@
+bleak==2.1.1
+dbus-fast==4.0.0
+python-dotenv==1.2.2

src/Raspberry Pi/ble_servo_controller.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+"""
+Servo Controller via BLE for ESP32
+Controls 32 servos through Bluetooth Low Energy connection to ESP32
+
+Hardware setup:
+- ESP32 with BLE characteristic: 3c5454f6-b1f7-4206-89f9-04677f4f467d
+- Depth matrix from vision system converted to servo angles
+"""
+
+import asyncio
+import os
+import numpy as np
+from dotenv import load_dotenv
+from bleak import BleakClient
+
+# --- CONFIGURATION ---
+load_dotenv()
+ADDRESS_MAC = os.getenv("ESP32_MAC_ADRESS") or os.getenv("ESP32_MAC_ADDRESS")
+CHARACTERISTIC_UUID = "3c5454f6-b1f7-4206-89f9-04677f4f467d"
+
+# -- PARAMETERS
+MAX_SERVOS = 36
+ANGLE_MAX_HARDWARE = 180
+MULTIPLIER_ANGLE = 90
+SLEEP_BETWEEN_COMMANDS = 0.05
+SMOOTH_STEP_SIZE = 5
+SMOOTH_STEP_DELAY = 0.02
+DEPTH_PATCHES = np.array([
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00],
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00],
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00],
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00],
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00],
+    [0.00, 0.00, 0.00, 0.00, 0.00, 0.00]
+])
+
+
+class BleServoController:
+    """Control servos via BLE connection to ESP32."""
+
+    def __init__(self, client):
+        self.client = client
+        self.positions_history = {}
+
+    def _convert_to_angle(self, normalized_value):
+        """Convert normalized depth value to servo angle."""
+        angle = int(normalized_value * MULTIPLIER_ANGLE)
+        return max(0, min(ANGLE_MAX_HARDWARE, angle))
+
+    async def send_servo_command(self, servo_id, angle):
+        """Send servo command via BLE."""
+        payload = bytearray([angle, servo_id])
+        try:
+            await self.client.write_gatt_char(CHARACTERISTIC_UUID, payload)
+            self.positions_history[servo_id] = angle
+            await asyncio.sleep(SLEEP_BETWEEN_COMMANDS)
+        except Exception as e:
+            print(f"[-] Failed to send Servo {servo_id}: {e}")
+
+    async def move_smooth(self, servo_id, current_angle, target_angle):
+        """Move servo smoothly from current to target angle."""
+        if current_angle == -1:
+            await self.send_servo_command(servo_id, target_angle)
+            return
+
+        delta = target_angle - current_angle
+        steps = abs(delta) // SMOOTH_STEP_SIZE
+        direction = 1 if delta > 0 else -1
+
+        for i in range(1, steps + 1):
+            intermediate = current_angle + direction * SMOOTH_STEP_SIZE * i
+            await self.send_servo_command(servo_id, intermediate)
+            await asyncio.sleep(SMOOTH_STEP_DELAY)
+
+        if self.positions_history.get(servo_id) != target_angle:
+            await self.send_servo_command(servo_id, target_angle)
+
+    async def process_matrix(self, matrix):
+        """Process depth matrix and update servo positions."""
+        rows, cols = matrix.shape
+
+        for r in range(rows):
+            for c in range(cols):
+                servo_id = r * cols + c
+
+                if servo_id >= MAX_SERVOS:
+                    continue
+
+                target_angle = self._convert_to_angle(matrix[r, c])
+                last_angle = self.positions_history.get(servo_id, -1)
+
+                if target_angle != last_angle:
+                    await self.move_smooth(servo_id, last_angle, target_angle)
+
+
+async def run_sync_process(matrix):
+    """Connect to ESP32 via BLE and synchronize servo positions."""
+    if not ADDRESS_MAC:
+        print("[!] Error: ESP32_MAC_ADDRESS is missing from .env")
+        return
+
+    print(f"[*] Attempting to connect : {ADDRESS_MAC}...")
+
+    try:
+        async with BleakClient(ADDRESS_MAC) as client:
+            print("[+] Bluetooth Connected.")
+            controller = BleServoController(client)
+            await controller.process_matrix(matrix)
+            print("[+] Synchronization complete.")
+    except Exception as e:
+        print(f"[!] Connection error : {e}")
+
+
+if __name__ == "__main__":
+    # Launch asyncio loop with test depth matrix
+    asyncio.run(run_sync_process(DEPTH_PATCHES))

src/SERVER.md

@@ -0,0 +1,66 @@
+# ESP32 Bluetooth Servo Matrix Controller
+
+This Python script synchronizes a numerical matrix (NumPy) with a physical grid of servomoteurs via **Bluetooth Low Energy (BLE)**. It maps normalized values (0.0 to 1.0) to physical angles and transmits them to an ESP32 equipped with a PCA9685 PWM driver.
+
+## ⚙️ How It Works
+
+### 1. Data Mapping & Logic
+
+The script processes a 2D NumPy array where each cell represents a specific servo motor.
+
+* **Index Calculation**: Servos are addressed sequentially. For a matrix cell at `[row, col]`, the Servo ID is calculated as: $ID = row \times total\_columns + column$.
+
+
+* 
+**Angle Conversion**: Normalized values (0.0 to 1.0) are multiplied by a `MULTIPLIER_ANGLE` (default: 90) to determine the target degrees.
+
+
+* 
+**Hardware Safety**: Angles are strictly clamped between 0° and 180° to prevent mechanical damage to the servos.
+
+
+
+### 2. Communication Protocol
+
+The script communicates with the ESP32 using the **GATT (Generic Attribute Profile)** protocol over BLE.
+
+* 
+**Packet Structure**: Commands are sent as a 2-byte `bytearray`:
+
+
+* **Byte 0**: Target Angle ($0$ to $180$).
+* **Byte 1**: Servo ID ($0$ to $15$).
+
+
+* 
+**Targeting All**: While not used by default in the matrix loop, sending `255` as the Servo ID triggers the "All Servos" mode on the firmware.
+
+
+
+### 3. Efficiency & Fluidity Features
+
+To ensure smooth movement and prevent Bluetooth congestion:
+
+* **Differential Updates**: The `ServoController` maintains a `positions_history` dictionary. It only transmits a command if the new angle differs from the last sent position.
+* 
+**Command Throttling**: A micro-delay (`0.01s`) is inserted between each BLE write. This prevents the PCA9685 and the ESP32 serial buffer from being overwhelmed, reducing "jitter" or buzzing sounds.
+
+
+* 
+**Connection Management**: The script uses an asynchronous context manager (`BleakClient`) to ensure the connection is cleanly opened and closed.
+
+
+
+## 🛠 Project Structure
+
+| Component | Description |
+| --- | --- |
+| **`ServoController`** | Class managing state, angle conversion, and BLE transmissions. |
+| **`process_matrix`** | Iterates through the NumPy array and filters out indices exceeding `MAX_SERVOS`. |
+| **`.env` File** | Stores the `ESP32_MAC_ADRESS` to keep the code portable and secure. |
+
+## 🚀 Setup
+
+1. **Environment**: Ensure `bleak`, `numpy`, and `python-dotenv` are installed.
+2. **Configuration**: Update your `.env` file with your ESP32's MAC address.
+3. **Execution**: Run the script. It will scan, connect, and move the servos according to `TEST_DATA`.

src/controller.ino

@@ -0,0 +1,142 @@
+#include <Wire.h>
+#include <Adafruit_PWMServoDriver.h>
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+
+#define SERVICE_UUID        "f46d35c6-518c-44d4-8fe4-bba375eea5a9"
+#define CHARACTERISTIC_UUID "3c5454f6-b1f7-4206-89f9-04677f4f467d"
+
+#define SERVO_MIN 150
+#define SERVO_MAX 600
+#define NB_SERVOS 36
+
+Adafruit_PWMServoDriver pwmA = Adafruit_PWMServoDriver(0x40);
+Adafruit_PWMServoDriver pwmB = Adafruit_PWMServoDriver(0x41);
+Adafruit_PWMServoDriver pwmC = Adafruit_PWMServoDriver(0x42);
+bool deviceConnected = false;
+bool oldDeviceConnected = false;
+
+uint16_t angleToPulse(int angle) {
+  return map(angle, 0, 180, SERVO_MIN, SERVO_MAX);
+}
+
+void moveServo(int channel, int angle) {
+  if (channel < 0 || channel >= NB_SERVOS) {
+    return;
+  }
+
+  uint16_t pulse = angleToPulse(angle);
+
+  if (channel < 16) {
+    pwmA.setPWM(channel, 0, pulse);
+  } else if (channel < 32) {
+    pwmB.setPWM(channel - 16, 0, pulse);
+  } else {
+    pwmC.setPWM(channel - 32, 0, pulse);
+  }
+}
+
+void applyAngleToServos(const uint8_t* data, size_t length) {
+  if (length < 2 || data == NULL) {
+    return;
+  }
+
+  int angle = data[0];
+  if (angle > 180) {
+    angle = 180;
+  }
+
+  Serial.print("[Action] Angle : ");
+  Serial.print(angle);
+  Serial.print("° on Servo : ");
+
+  for (size_t index = 1; index < length; index++) {
+    uint8_t servoNumber = data[index];
+
+    if (servoNumber == 255) {
+      Serial.print("ALL ");
+      for (int channel = 0; channel < NB_SERVOS; channel++) {
+        moveServo(channel, angle);
+      }
+    } else if (servoNumber < NB_SERVOS) {
+      Serial.print(servoNumber);
+      Serial.print(" ");
+      moveServo(servoNumber, angle);
+    }
+  }
+
+  Serial.println();
+}
+
+class MyServerCallbacks : public BLEServerCallbacks {
+  void onConnect(BLEServer* server) {
+    (void)server;
+    deviceConnected = true;
+    Serial.println("\n>>> Device CONNECTED !");
+  }
+
+  void onDisconnect(BLEServer* server) {
+    (void)server;
+    deviceConnected = false;
+    Serial.println("\n<<< Device DISCONNECTED.");
+  }
+};
+
+class MyCallbacks : public BLECharacteristicCallbacks {
+  void onWrite(BLECharacteristic* characteristic) {
+    size_t length = characteristic->getLength();
+    uint8_t* data = characteristic->getData();
+    applyAngleToServos(data, length);
+  }
+};
+
+void setup() {
+  Serial.begin(115200);
+
+  Serial.println("\n--------------------------------------------------");
+  Serial.println("--- SYSTEM INITIALISATION ---");
+
+  Wire.begin();
+  pwmA.begin();
+  pwmA.setPWMFreq(50);
+  pwmB.begin();
+  pwmB.setPWMFreq(50);
+  pwmC.begin();
+  pwmC.setPWMFreq(50);
+  Serial.println("1. PCA9685 #1 (0x40): OK (Frequence 50Hz)");
+  Serial.println("2. PCA9685 #2 (0x41): OK (Frequence 50Hz)");
+  Serial.println("3. PCA9685 #3 (0x42): OK (Frequence 50Hz)");
+
+  BLEDevice::init("Sensora Device");
+  BLEServer *pServer = BLEDevice::createServer();
+  pServer->setCallbacks(new MyServerCallbacks());
+
+  BLEService *pService = pServer->createService(SERVICE_UUID);
+  BLECharacteristic *pChar = pService->createCharacteristic(
+    CHARACTERISTIC_UUID,
+    BLECharacteristic::PROPERTY_WRITE
+  );
+
+  pChar->setCallbacks(new MyCallbacks());
+  pService->start();
+
+  BLEDevice::getAdvertising()->start();
+
+  Serial.println("4. Bluetooth : OK ('Sensora Device')");
+  Serial.println("5. Status : Waiting for connexion...");
+  Serial.println("--------------------------------------------------");
+}
+
+void loop() {
+  if (!deviceConnected && oldDeviceConnected) {
+    delay(500);
+    BLEDevice::getAdvertising()->start();
+    Serial.println("... Advertising restarted (Visible) ...");
+    oldDeviceConnected = deviceConnected;
+  }
+
+  if (deviceConnected && !oldDeviceConnected) {
+    oldDeviceConnected = deviceConnected;
+  }
+}