realse all fingers at same time

Author: violetforest

Bluetooth-Arduino-Prosthetic-Control.ino

@@ -67,7 +67,7 @@ void setup() {
 
   handServos.setupServos();
 
-  BLEDevice::init("Brunel Hand");
+  BLEDevice::init("Brunel Hand Right");
   BLEServer *pServer = BLEDevice::createServer();
   pServer->setCallbacks(new OnConnectCallback());
 
@@ -162,7 +162,7 @@ void updateCounter() {
   if (buttonState == LOW) {
       holdTime = millis() - startPressed;
       if (holdTime >= 1100) {
-        handServos.moveServos2("0,180,180,180");
+        handServos.openFingers();
       }
   } else {
       idleTime = millis() - endPressed;

HandServo.cpp

@@ -0,0 +1,55 @@
+#include "Arduino.h"
+#include "HandServo.h"
+
+HandServo::HandServo(int interval)
+{
+  updateInterval = interval;
+  increment = 1;
+}
+
+void HandServo::setupServo(int pin) {
+  servo.attach(pin);
+}
+
+void HandServo::moveServo() {  
+  if((millis() - lastUpdate) > updateInterval)  // time to update
+  {
+    lastUpdate = millis();
+    pos += increment;
+    servo.write(pos);
+
+    if ((pos >= 180) || (pos <= 0)) // end of sweep
+    {
+      // reverse direction
+      increment = -increment;
+    }
+  }
+
+//  if((millis() - lastUpdate) > updateInterval) {
+//    lastUpdate = millis();
+//    Serial.println("move");
+//    if (lastPos > newPos) {
+//       for (int i = lastPos; i > newPos; i--) {
+//          servo.write(i);
+//          lastPos = servo.read();
+//          delay(15);
+//       }
+//      } else if (lastPos < newPos) {
+//        for (int i = lastPos; i < newPos; i++) {
+//          servo.write(i);
+//          lastPos = servo.read();
+//          delay(15);
+//        }
+//      }
+//  }
+}
+
+void HandServo::calibrate() {
+  int pos = lastPos;
+  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
+    // in steps of 1 degree
+      servo.write(pos);
+      delay(15);             // waits 15ms for the servo to reach the position
+  }
+  lastPos = servo.read();
+}

HandServo.h

@@ -0,0 +1,26 @@
+#ifndef HandServo_h
+#define HandServo_h
+
+#include "Arduino.h"
+#include <ESP32Servo.h>
+
+class HandServo
+{
+  public:
+    HandServo(int);
+    Servo servo;
+    void setupServo(int pin);
+    void moveServo();
+    void calibrate();
+    int updateInterval;      // interval between updates
+    int increment = 1;        // increment to move for each interval    
+
+  private:
+    int pin;
+    unsigned long lastUpdate; // last update of position
+    int pos;              // current servo position
+    int interval;
+    int lastPos = 0;
+};
+
+#endif

HandServos.cpp

@@ -70,50 +70,6 @@ void HandServos::openThumb() {
   lastPosT = servoT.read();
 }
 
-void HandServos::openFingers() {
-  if (lastPosI < 180) {
-    for (int i = lastPosI; i <= 180; i += 1) {
-      // in steps of 1 degree
-      Serial.println(i);
-      servoI.write(i);
-      lastPosI = i;
-      Serial.println(i);
-      delay(15);
-    }
-  } else {
-    lastPosI = 0;
-  }
-
-  if (lastPosM < 180) {
-    for (int i = lastPosM; i <= 180; i += 1) {
-      // in steps of 1 degree
-      Serial.println(i);
-      servoM.write(i);
-      lastPosM = i;
-      Serial.println(i);
-      delay(15);
-    }
-  } else {
-    lastPosM = 0;
-  }
-
-  if (lastPosR < 180) {
-    for (int i = lastPosR; i <= 180; i += 1) {
-      // in steps of 1 degree
-      Serial.println(i);
-      servoR.write(i);
-      lastPosR = i;
-      Serial.println(i);
-      delay(15);
-    }
-  } else {
-    lastPosR = 0;
-  }
-  lastPosI = servoI.read();
-  lastPosM = servoM.read();
-  lastPosR = servoR.read();
-}
-
 void HandServos::calibrate() {
   int pos = startPosI;
   for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
@@ -273,3 +229,44 @@ void HandServos::moveServos2(String input) {
      }
     } 
 }
+
+void HandServos::openFingers() {
+    Serial.println("opening");
+    int angleT = servoT.read();
+    int angleI = servoI.read();
+    int angleM = servoM.read();
+    int angleR = servoR.read();
+    
+    int angleStep = 10;
+
+    angleT = angleT - angleStep;
+    angleI = angleI + angleStep;
+    angleM = angleM + angleStep;
+    angleR = angleR + angleStep;
+  
+    if (angleT <= 0) {
+      angleT = 0;
+    }
+    
+    if (angleI >= 180) {
+      angleI = 180;
+    }
+
+    if (angleM >= 180) {
+      angleM = 180;
+    }
+
+    if (angleR >= 180) {
+      angleR = 180;
+    }
+
+    servoT.write(angleT); // move the servo to desired angle
+    servoI.write(angleI); // move the servo to desired angle
+    servoM.write(angleM); // move the servo to desired angle
+    servoR.write(angleR); // move the servo to desired angle
+
+    lastPosT = servoT.read();
+    lastPosI = servoI.read();
+    lastPosM = servoM.read();
+    lastPosR = servoR.read();
+}