Allow Driving FFB Servo's through PCA9685

firmware/lucidgloves-firmware/AdvancedConfig.h

@@ -66,3 +66,13 @@
 #if defined(ESP32) && ESP32_DUAL_CORE
   #define ESP32_DUAL_CORE_SET true
 #endif
+
+
+
+//Advanced options relating to I2C and the PCA9685
+#if SERVO_INTERFACE == SERVO_PCA9685
+  #define PWM_Board_0_I2C_ADDRESS       0x40    //The I2C address of the PCA9685 Board. Default is 0x40
+  #define PWM_Board_0_PWM_FREQUENCY     50      //set the PWM frequency the board uses. Default is 50hz (20ms Cycles) which is what most analogue servos use
+  #define ServoMin_uS                   500     // Min microsecond pulse length, moves servo to 0* or fully retracted position. Value for sg90 and mg90s by default (ESP32Servo default is 500)
+  #define ServoMax_uS                   2500    // Max microsecond pulse length, moves servo to 180* or whatever it's max rotation position is. Value for sg90 and mg90s by default (ESP32Servo default is 2500)
+#endif

firmware/lucidgloves-firmware/ConfigUtils.h

@@ -45,6 +45,10 @@ class ordered_lock {
 #define COMM_SERIAL 0   
 #define COMM_BTSERIAL 1 
 
+//ServoInterface
+#define SERVO_DIRECT 0
+#define SERVO_PCA9685 1 
+
 //Encode
 #define ENCODE_LEGACY 0
 #define ENCODE_ALPHA  1

firmware/lucidgloves-firmware/I2C.h

@@ -0,0 +1,9 @@
+#pragma once
+#include "AdvancedConfig.h"
+#include <Wire.h>
+
+
+void Setup_I2C() {
+      Wire.begin(PIN_I2C_SDA, PIN_I2C_SCL);
+      Serial.println("I2C Initialized");
+}

firmware/lucidgloves-firmware/IServo.ino

@@ -0,0 +1,9 @@
+//Interface for Servo Connections
+
+class IServo {
+
+  public:
+    virtual bool isReady() = 0;
+
+    virtual void InitServoInterface() = 0;
+};

firmware/lucidgloves-firmware/ServoDirectGPIO.ino

@@ -0,0 +1,27 @@
+//only compiles if needed
+#if USING_FORCE_FEEDBACK && SERVO_INTERFACE == SERVO_DIRECT
+  #pragma once
+  #if defined(ESP32)
+    #include <ESP32Servo.h>
+  #else
+    #include <Servo.h>
+  #endif
+
+class DirectServoConnection : public IServo {
+  private:
+    bool m_isReady;
+
+  public:
+    DirectServoConnection() {
+      m_isReady = false;
+    }
+
+    bool isReady(){
+      return m_isReady;
+    }
+
+    void InitServoInterface(){
+      m_isReady = true;
+    }
+};
+#endif

firmware/lucidgloves-firmware/ServoPCA9685.ino

@@ -0,0 +1,25 @@
+//only compiles if needed
+#if USING_FORCE_FEEDBACK && SERVO_INTERFACE == SERVO_PCA9685
+#include "I2C.h"
+
+class PCA9685ServoConnection : public IServo 
+{
+  private:
+    bool m_isReady;
+
+  public:
+    PCA9685ServoConnection() {
+      m_isReady = false;
+    }
+
+    bool isReady(){
+      return m_isReady;
+    }
+
+    void InitServoInterface(){
+      Setup_I2C();
+      Initialize_PCA9685_Board();
+      m_isReady = true;
+    }
+};
+#endif

firmware/lucidgloves-firmware/_PCA9685Servo.ino

@@ -0,0 +1,57 @@
+//only compiles if needed
+#if USING_FORCE_FEEDBACK && SERVO_INTERFACE == SERVO_PCA9685
+#include <Adafruit_PWMServoDriver.h>
+
+#define MIN_PULSE_WIDTH ServoMin_uS   //Sets the MIN_PULSE_WIDTH setting used by smooth stepping to your configured ServoMin (Default in esp32servo is 500)
+#define MAX_PULSE_WIDTH ServoMax_uS   //Sets the Max_PULSE_WIDTH setting used by smooth stepping to your configured ServoMax (Default in esp32servo is 2500)
+
+Adafruit_PWMServoDriver pwm_board_0 = Adafruit_PWMServoDriver(PWM_Board_0_I2C_ADDRESS, Wire);
+
+int Initialize_PCA9685_Board()
+{
+  pwm_board_0.begin();
+  pwm_board_0.setOscillatorFrequency(25000000);
+  pwm_board_0.setPWMFreq(PWM_Board_0_PWM_FREQUENCY);  // Analog servos usually run at ~50 Hz updates
+  Serial.println("PCA9685 Board Initialized ");
+}
+
+class Servo
+{
+public:
+  Servo();
+  int attach(int pin);            // input to match servo.h & esp32servo.h format
+  void write(int value);          // Write as an angle (converted into microsecond pulse below)
+  void writeMicroseconds(int value);  // Write to servo directly as an "X" microsecond pulse
+
+private:
+  int minMicroSeconds = ServoMin_uS;                  // microsecond value to move servo to 0* or fully retracted position  
+  int maxMicroSeconds = ServoMax_uS;                  // microsecond value to move servo to 180* or whatever it's max rotation position is  
+  int driverChannel = 0;                          // driverboard channel connected to the servo
+};
+
+Servo::Servo()
+{}
+
+int Servo::attach(int pin)
+{
+  this->driverChannel = pin;
+}
+
+void Servo::write(int value)
+{
+  {
+  if (value < 0)
+      value = 0;
+  else if (value > 180)
+      value = 180;
+
+  value = map(value, 0, 180, this->minMicroSeconds, this->maxMicroSeconds);
+  }
+  this->writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value){
+    pwm_board_0.writeMicroseconds(this->driverChannel, value);
+    //Serial.println("Servo called!");
+}
+#endif

firmware/lucidgloves-firmware/haptics.ino

@@ -1,18 +1,23 @@
 #if USING_FORCE_FEEDBACK
 
-#if defined(ESP32)
-  #include "ESP32Servo.h"
-#else
-  #include "Servo.h"
-#endif
-
 Servo pinkyServo;
 Servo ringServo;
 Servo middleServo;
 Servo indexServo;
 Servo thumbServo;
 
+IServo* servoint;
+
 void setupServoHaptics(){
+
+  #if SERVO_INTERFACE == SERVO_DIRECT
+    servoint = new DirectServoConnection();
+  #elif SERVO_INTERFACE == SERVO_PCA9685
+    servoint = new PCA9685ServoConnection();
+  #endif
+
+  servoint->InitServoInterface(); //Calls the InitServoInterface() function. What that function does is determined by the defined servo interface
+
   pinkyServo.attach(PIN_PINKY_MOTOR);
   ringServo.attach(PIN_RING_MOTOR);
   middleServo.attach(PIN_MIDDLE_MOTOR);

firmware/lucidgloves-firmware/lucidgloves-firmware.ino

@@ -55,7 +55,18 @@
 
 #define USING_CALIB_PIN true //When PIN_CALIB is shorted (or it's button pushed) it will reset calibration if this is on.
 
+//servo Configuration
 #define USING_FORCE_FEEDBACK false //Force feedback haptics allow you to feel the solid objects you hold
+#define SERVO_INTERFACE SERVO_DIRECT  //How your servos are connected. Options are: SERVO_DIRECT (mcu gpio pins), SERVO_PCA9685 (through I2C PCA9685 board)
+  //servos through gpio Pins
+
+  //servos through PCA9685
+    // To use you must install the Adafruit PCA9685 PWM Servo Driver Library 
+    // https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
+  #if SERVO_INTERFACE == SERVO_PCA9685
+    #define PIN_I2C_SDA         -1     //Pin to use for the I2C SDA line connected to the PCA9685
+    #define PIN_I2C_SCL         -1     //Pin to use for the I2C SCL line connected to the PCA9685
+  #endif
 #define FLIP_FORCE_FEEDBACK true
 #define SERVO_SCALING false //dynamic scaling of servo motors
 
@@ -77,7 +88,7 @@
   #define PIN_PNCH_BTN  23 //unused if gesture set
   #define PIN_CALIB     32 //button for recalibration (You can set this to GPIO0 to use the BOOT button, but only when using Bluetooth.)
   #define DEBUG_LED 2
-  #define PIN_PINKY_MOTOR     19  //used for force feedback
+  #define PIN_PINKY_MOTOR     19  //used for force feedback **alternatively this is which board channel you are plugged into on the PCA9685 servodriver board (accepted values 0-15 in that case)**
   #define PIN_RING_MOTOR      18 //^
   #define PIN_MIDDLE_MOTOR    5 //^
   #define PIN_INDEX_MOTOR     17 //^
@@ -128,7 +139,7 @@
   #define PIN_PNCH_BTN  12 //unused if gesture set
   #define PIN_CALIB     13 //button for recalibration
   #define DEBUG_LED     LED_BUILTIN
-  #define PIN_PINKY_MOTOR     2 //used for force feedback
+  #define PIN_PINKY_MOTOR     2 //used for force feedback **alternatively this is which board channel you are plugged into on the PCA9685 servodriver board (accepted values 0-15 in that case)**
   #define PIN_RING_MOTOR      3 //^
   #define PIN_MIDDLE_MOTOR    4 //^
   #define PIN_INDEX_MOTOR     5 //^