#include #include #include "ManagedServo.h" #include "Finger.h" #include "Thumb.h" #include "Wrist.h" #include "WiFiNINA.h" // ----- Servo Setup ----- #define NUM_SERVOS 18 #define SERVO_INDEX_LOWER 15 #define SERVO_INDEX_UPPER 1 #define SERVO_MIDDLE_LOWER 2 #define SERVO_MIDDLE_UPPER 3 #define SERVO_RING_LOWER 4 #define SERVO_RING_UPPER 5 #define SERVO_PINKY_LOWER 6 #define SERVO_PINKY_UPPER 7 #define SERVO_INDEX_TIP 8 #define SERVO_MIDDLE_TIP 9 #define SERVO_RING_TIP 10 #define SERVO_PINKY_TIP 11 #define SERVO_THUMB_TIP 12 #define SERVO_THUMB_RIGHT 13 #define SERVO_THUMB_LEFT 14 #define SERVO_THUMB_ROTATE 0 #define SERVO_WRIST_L 16 #define SERVO_WRIST_R 17 #define DEMO_BUTTON 19 // Used for an optional external button to allow the hand to run through a series of canned poses // The table below defines the servo parameters for each of the servos in the hand. // You may need to adjust these if you are using different servos, or wire the // servos to different GPIO pins. ManagedServo managedServos[NUM_SERVOS] = { // Servo GPIO Pin, Min, Max, Default, Inverted ManagedServo(20, 30, 140, 30, false), // Index Lower 0 ManagedServo(18, 30, 130, 30, true), // Index Upper 1 ManagedServo(17, 30, 130, 30, false), // Middle Lower 2 ManagedServo(16, 50, 140, 40, true), // Middle Upper 3 ManagedServo(2, 35, 145, 30, true), // Ring Lower 4 ManagedServo(3, 35, 135, 30, false), // Ring Upper 5 ManagedServo(4, 30, 140, 30, true), // Pinky Lower 6 ManagedServo(5, 30, 140, 30, false), // Pinky Upper 7 ManagedServo(15, 30, 120, 30, false), // Index Tip 8 ManagedServo(14, 30, 120, 30, false), // Middle Tip 9 ManagedServo(6, 30, 150, 30, true), // Ring Tip 10 ManagedServo(7, 30, 150, 30, true), // Pinky Tip 11 ManagedServo(13, 30, 130, 30, false), // Thumb Tip 12 ManagedServo(8, 10, 70, 30, false), // Thumb Right 13 ManagedServo(10, 20, 140, 20, true), // Thumb Left 14 ManagedServo(12, 30,120, 30, false), // Thumb Rotate 15 ManagedServo(9, 20, 130, 75, false), // Wrist Left 16 ManagedServo(11, 20, 130, 75, false) // Wrist Right 17 }; // Finger, Thumb, and Wrist objects for managing the DOF's in a more intuitive fashion. typedef enum fingerIdx { FINGER_INDEX, FINGER_MIDDLE, FINGER_RING, FINGER_PINKY, NUM_FINGERS } FINGER_IDX; Finger fingers[NUM_FINGERS] = { Finger("index", managedServos[SERVO_INDEX_LOWER], managedServos[SERVO_INDEX_UPPER], managedServos[SERVO_INDEX_TIP]), Finger("middle", managedServos[SERVO_MIDDLE_LOWER], managedServos[SERVO_MIDDLE_UPPER], managedServos[SERVO_MIDDLE_TIP]), Finger("ring", managedServos[SERVO_RING_LOWER], managedServos[SERVO_RING_UPPER], managedServos[SERVO_RING_TIP]), Finger("pinky", managedServos[SERVO_PINKY_LOWER], managedServos[SERVO_PINKY_UPPER], managedServos[SERVO_PINKY_TIP]) }; Thumb thumb(managedServos[SERVO_THUMB_LEFT], managedServos[SERVO_THUMB_RIGHT], managedServos[SERVO_THUMB_TIP], managedServos[SERVO_THUMB_ROTATE]); Wrist wrist(managedServos[SERVO_WRIST_L], managedServos[SERVO_WRIST_R]); // ----- BLE Setup ----- // The device implments the Nordic UART service to use as a general purpose command // channel, and a custom service to use for streaming the DOF angles for the joints // in the hand. // UART service BLEService uartService("6E400001-B5A3-F393-E0A9-E50E24DCCA9E"); // UART service // UART characteristics BLECharacteristic txCharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E", BLENotify, 20); BLECharacteristic rxCharacteristic("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWrite, 20); // Custom DOF streaming service BLEService dofService("1e16c1b4-1936-4f0e-ab62-5e0a702a4935"); // Custom DOF streaming characteristics BLECharacteristic dofCharacteristic("1e16c1b5-1936-4f0e-ab62-5e0a702a4935", BLEWriteWithoutResponse, 20); // Heartbeat timer uint32_t heartbeat = 0; UniversalTimer heartbeatTimer(5000, true); // 5 second message interval // Connection timeout timer UniversalTimer connectionTimeout(10000, true); // 10 second timeout // Reset servos to default position void setDefaultPose() { for (int index = 0; index < NUM_SERVOS; index++) { managedServos[index].setServoPosition(managedServos[index].getDefaultPosition()); } } // -- Canned Procedural Poses for Testing ----------------------- // Hand pose for countdown - all fingers closed void setZeroPose() { setDefaultPose(); // Move thumb to lower closed position thumb.setMaxPosition(); // Move all fingers to max position for (int finger = 0; finger < NUM_FINGERS; finger++) { fingers[finger].setMaxPosition(); fingers[finger].update(); } } // Hand pose for countdown - one finger open void setOnePose() { setDefaultPose(); // Move thumb to lower closed position thumb.setMaxPosition(); // Move all fingers other than index to max position for (int finger = 1; finger < NUM_FINGERS; finger++) { fingers[finger].setMaxPosition(); fingers[finger].update(); } } // Hand pose for countdown - two fingers open void setTwoPose() { setDefaultPose(); // Move thumb to lower closed position thumb.setMaxPosition(); // Move all fingers other than index,middle finger to max position for (int finger = 2; finger < NUM_FINGERS; finger++) { fingers[finger].setMaxPosition(); fingers[finger].update(); } } // Hand pose for countdown - three fingers open void setThreePose() { setDefaultPose(); // Move thumb to lower closed position thumb.setMaxPosition(); // Move all fingers other than index,middle,ring finger to max position for (int finger = 3; finger < NUM_FINGERS; finger++) { fingers[finger].setMaxPosition(); fingers[finger].update(); } } // Hand pose for countdown - four fingers open void setFourPose() { setDefaultPose(); // Move thumb to lower closed position thumb.setMaxPosition(); } // Countdown and back up void count() { // Move thumb to lower closed position managedServos[SERVO_THUMB_TIP].moveToMaxPosition(); managedServos[SERVO_THUMB_RIGHT].moveToMinPosition(); managedServos[SERVO_THUMB_LEFT].moveToMaxPosition(); delay(200); setZeroPose(); delay(1000); setOnePose(); delay(1000); setTwoPose(); delay(1000); setThreePose(); delay(1000); setFourPose(); delay(1000); setDefaultPose(); delay(1000); setFourPose(); delay(1000); setThreePose(); delay(1000); setTwoPose(); delay(1000); setOnePose(); delay(1000); setZeroPose(); delay(1000); setDefaultPose(); } // Waves the hand side to side void wave() { setDefaultPose(); for (int cycle = 0; cycle < 5; ++cycle) { for (int yaw = wrist.getYawMin(); yaw <= wrist.getYawMax(); yaw ++) { wrist.setYaw(yaw); wrist.update(); delay(6); } for (int yaw = wrist.getYawMax(); yaw >= wrist.getYawMin(); yaw --) { wrist.setYaw(yaw); wrist.update(); delay(6); } } delay(6); setDefaultPose(); } // Perform a shaka void shaka() { const int SHAKA_RANGE = 20; setDefaultPose(); for (int finger = FINGER_INDEX; finger < FINGER_PINKY; ++finger) { fingers[finger].setMaxPosition(); fingers[finger].update(); } thumb.setPitch(0); thumb.setYaw(0); thumb.setRoll(15); thumb.update(); for (int cycle = 0; cycle < 5; ++cycle) { for (int yaw = -SHAKA_RANGE; yaw <= SHAKA_RANGE; yaw ++) { wrist.setYaw(yaw); wrist.update(); delay(6); } for (int yaw = SHAKA_RANGE; yaw >= -SHAKA_RANGE; yaw --) { wrist.setYaw(yaw); wrist.update(); delay(6); } } setDefaultPose(); } void thumbRangeTest() { // Cycle through the two thumb servos testing the range for (int right = managedServos[SERVO_THUMB_RIGHT].getMinPosition(); right <= managedServos[SERVO_THUMB_RIGHT].getMaxPosition(); right+=5) { managedServos[SERVO_THUMB_RIGHT].setServoPosition(right); for (int left = managedServos[SERVO_THUMB_LEFT].getMinPosition(); left <= managedServos[SERVO_THUMB_LEFT].getMaxPosition(); left+=5) { managedServos[SERVO_THUMB_LEFT].setServoPosition(left); delay(100); } } setDefaultPose(); } void defaultFingers() { for (int finger = FINGER_INDEX; finger <= FINGER_PINKY; ++finger) { fingers[finger].setExtension(100); fingers[finger].update(); } thumb.setExtension(100); thumb.update(); } void fingerTest() { defaultFingers(); delay(1000); // Pinky fingers[FINGER_PINKY].setExtension(25); fingers[FINGER_PINKY].update(); thumb.setPitch(60); thumb.setYaw(0); thumb.setFlexion(45); thumb.setRoll(0); thumb.update(); delay(1000); defaultFingers(); delay(1000); // Ring fingers[FINGER_RING].setExtension(30); fingers[FINGER_RING].update(); thumb.setPitch(60); thumb.setYaw(0); thumb.setFlexion(30); thumb.setRoll(0); thumb.update(); delay(1000); defaultFingers(); delay(1000); // Middle fingers[FINGER_MIDDLE].setExtension(35); fingers[FINGER_MIDDLE].update(); thumb.setPitch(50); thumb.setYaw(0); thumb.setFlexion(40); thumb.setRoll(0); thumb.update(); delay(1000); defaultFingers(); delay(1000); // Index fingers[FINGER_INDEX].setExtension(35); fingers[FINGER_INDEX].update(); thumb.setPitch(40); thumb.setYaw(0); thumb.setFlexion(45); thumb.setRoll(0); thumb.update(); delay(1000); defaultFingers(); delay(1000); } // Dump out the current DOF angles String printDOFS() { // This command outputs a JSON array of all of the angle ranges for the DOFS in the hand String result = "DOFS:[ "; // Would be nicer if this could be more automated based on the data structures, but for // now we just output the values in the right order for (int finger = 0; finger < NUM_FINGERS; ++finger) { // Fingers result += "{ \"name\": \""; result += fingers[finger].getName(); result += "_pitch\", \"range\": ["; result += fingers[finger].getPitchMin(); result += ", "; result += fingers[finger].getPitchMax(); result += "] }, "; result += "{ \"name\": \""; result += fingers[finger].getName(); result += "_yaw\", \"range\": ["; result += fingers[finger].getYawMin(); result += ", "; result += fingers[finger].getYawMax(); result += "] }, "; result += "{ \"name\": \""; result += fingers[finger].getName(); result += "_flexion\", \"range\": ["; result += fingers[finger].getFlexionMin(); result += ", "; result += fingers[finger].getFlexionMax(); result += "] }, "; } // Thumb result += "{ \"name\": \"thumb_pitch\", \"range\": ["; result += thumb.getPitchMin(); result += ", "; result += thumb.getPitchMax(); result += "] }, "; result += "{ \"name\": \"thumb_yaw\", \"range\": ["; result += thumb.getYawMin(); result += ", "; result += thumb.getYawMax(); result += "] }, "; result += "{ \"name\": \"thumb_flexion\", \"range\": ["; result += thumb.getFlexionMin(); result += ", "; result += thumb.getFlexionMax(); result += "] }, "; // Roll is currently not used - this is here for when we want to enable it //result += "{ \"name\": \"thumb_roll\", \"range\": ["; //result += thumb.getRollMin(); //result += ", "; //result += thumb.getRollMax(); //result += "] }, "; // Wrist result += "{ \"name\": \"wrist_pitch\", \"range\": ["; result += wrist.getPitchMin(); result += ", "; result += wrist.getPitchMax(); result += "] }, "; result += "{ \"name\": \"wrist_yaw\", \"range\": ["; result += wrist.getYawMin(); result += ", "; result += wrist.getYawMax(); result += "] } "; result += "]"; return result; } // --- Main Setup ----------------------- void setup() { delay(15000); Serial.begin(9600); // initialize serial communication delay(2000); // ----- Servo Setup ----- for (int index = 0; index < NUM_SERVOS; index++) { managedServos[index].setupServo(); } // ----- BLE Setup ----- if (!BLE.begin()) { // initialize BLE Serial.println("starting BLE failed!"); while (1); } BLE.setConnectionInterval(6, 10); // Ask for a fast connection interval: 7.5 ms minimum, 12.5s maximum heartbeatTimer.start(); // Start heartbeat timer connectionTimeout.start(); // Start timeout BLE.setLocalName("DexHand"); // Set name for connection BLE.setAdvertisedService(uartService); // Add the service UUID uartService.addCharacteristic(rxCharacteristic); // Add the rxCharacteristic uartService.addCharacteristic(txCharacteristic); // Add the txCharacteristic dofService.addCharacteristic(dofCharacteristic); // Add the dofCharacteristic BLE.addService(uartService); // Add the service BLE.addService(dofService); // Add the service rxCharacteristic.setEventHandler(BLEWritten, rxHandler); // Assign event handler for characteristic dofCharacteristic.setEventHandler(BLEWritten, dofHandler); // Assign event handler for characteristic BLE.advertise(); // Start advertising Serial.println("Bluetooth device active, waiting for connections..."); setDefaultPose(); // ----- Demo Button Setup ----- pinMode(DEMO_BUTTON, INPUT_PULLUP); } // --- Main Loop and Processing ------------------------------- // Basic command parser for servo commands - nothing special, but it works // See the README.md for details on the commands and format void processCommand(String cmd) { cmd.toLowerCase(); // Split the string int colonPos = cmd.indexOf(':'); String cmdType; String servoIndex; String servoPosition; int index = 0; int position = 0; if (colonPos != -1) { cmdType = cmd.substring(0, colonPos); cmd = cmd.substring(colonPos + 1); colonPos = cmd.indexOf(':'); servoIndex = cmd.substring(0, colonPos); if (colonPos != -1) { servoPosition = cmd.substring(colonPos + 1); position = servoPosition.toInt(); } // Convert to integers index = servoIndex.toInt(); } else { // Single word command cmdType = cmd; cmdType.trim(); } Serial.print("CMD:"); Serial.print(cmdType); Serial.print(":"); Serial.print(index); Serial.print(":"); Serial.println(position); // Set the servo position if (cmdType == "set") { Serial.print("Setting Servo "); Serial.print(index); Serial.print(" to "); Serial.println(position); managedServos[index].setServoPosition(position); } if (cmdType == "max") { if (position != 0) { managedServos[index].setMaxPosition(position); } Serial.print("Setting Servo "); Serial.print(index); Serial.print(" to max"); managedServos[index].moveToMaxPosition(); } if (cmdType == "min") { if (position != 0) { managedServos[index].setMinPosition(position); } Serial.print("Setting Servo "); Serial.print(index); Serial.print(" to min"); managedServos[index].moveToMinPosition(); } if (cmdType == "fingermax") { if (index < NUM_FINGERS) { fingers[index].setMaxPosition(); fingers[index].update(); Serial.print("Setting finger "); Serial.print(index); Serial.print(" to max"); } } if (cmdType == "fingermin") { if (index < NUM_FINGERS) { fingers[index].setMinPosition(); fingers[index].update(); Serial.print("Setting finger "); Serial.print(index); Serial.print(" to min"); } } if (cmdType == "fingerextension") { // Accepts range from 0-100 where 0 is fully retracted toward palm, and 100 is fully extended away from palm if (index < NUM_FINGERS) { fingers[index].setExtension(position); fingers[index].update(); Serial.print("Setting finger "); Serial.print(index); Serial.print(" extension to "); Serial.println(position); } else if (index == NUM_FINGERS) { thumb.setExtension(position); thumb.update(); Serial.print("Setting thumb extension to "); Serial.println(position); } } if (cmdType == "wrist") { if (servoIndex == "pitch") { wrist.setPitch(position); wrist.update(); Serial.print("Setting wrist pitch to "); Serial.println(position); } if (servoIndex == "yaw") { wrist.setYaw(position); wrist.update(); Serial.print("Setting wrist yaw to "); Serial.println(position); } } if (cmdType == "thumb") { if (servoIndex == "pitch") { thumb.setPitch(position); thumb.update(); Serial.print("Setting thumb pitch to "); Serial.println(position); } if (servoIndex == "yaw") { thumb.setYaw(position); thumb.update(); Serial.print("Setting thumb yaw to "); Serial.println(position); } if (servoIndex == "flexion") { thumb.setFlexion(position); thumb.update(); Serial.print("Setting thumb flexion to "); Serial.println(position); } if (servoIndex == "roll") { thumb.setRoll(position); thumb.update(); Serial.print("Setting thumb roll to "); Serial.println(position); } } if (cmdType == "one") { setOnePose(); } if (cmdType == "two") { setTwoPose(); } if (cmdType == "three") { setThreePose(); } if (cmdType == "four") { setFourPose(); } if (cmdType == "default") { setDefaultPose(); } if (cmdType == "count") { count(); } if (cmdType == "wave") { wave(); } if (cmdType == "shaka") { shaka(); } if (cmdType == "thumbtest") { thumbRangeTest(); } if (cmdType == "fingertest") { fingerTest(); } if (cmdType == "hb") { connectionTimeout.resetTimerValue(); Serial.println("HB: Heartbeat received"); } if (cmdType == "gesture") { if (servoIndex == "count") { count(); } if (servoIndex == "wave") { wave(); } if (servoIndex == "shaka") { shaka(); } if (servoIndex == "reset") { setDefaultPose(); delay(500); } } if (cmdType == "dofs") { Serial.println(printDOFS().c_str()); } } // This method is called to process all of the higher level objects (Finger,Thumb) // and update the servo positions based on the current hand angles. void updateHand() { // Update fingers for (int index = 0; index < NUM_FINGERS; index++) { fingers[index].update(); } // Update thumb thumb.update(); // Update wrist wrist.update(); } void loop() { // ---- Serial Input Loop ----- // If there is no active BLE connection to the peripheral, then we will // process serial commands for debugging/tuning/testing etc. // Is there serial data available for input? if (Serial.available()) { String cmd = Serial.readStringUntil('\n'); Serial.print("Received CMD: "); Serial.println(cmd); processCommand(cmd); } // ----- BLE Loop ----- // If there is an active BLE connection to the peripheral, then we will // ignore serial processing and run in a tight loop where we receive // packets with the hand angles and convert those into servo positions BLEDevice central = BLE.central(); // wait for a BLE central if (central) { // if a central is connected to the peripheral: Serial.print("Connected to central - entering BLE streaming mode:"); Serial.println(central.address()); // print the central's MAC address // Start a fresh connection timeout timer connectionTimeout.resetTimerValue(); while (central.connected()) { // while the central is still connected to peripheral: // Check if it's time to send a heartbeat if (heartbeatTimer.check()) { heartbeat++; String data = "HB:" + String(heartbeat); txCharacteristic.writeValue(data.c_str()); } // Make sure we've received a heartbeat from the central recently if (connectionTimeout.check()) { Serial.println("Connection timeout - disconnecting"); central.disconnect(); break; } } Serial.print("Disconnected from central: "); Serial.println(central.address()); setDefaultPose(); } // ----- Demo Button Loop ----- // If the demo button is pressed, then we will run through a series of canned poses // to demonstrate the hand functionality if (digitalRead(DEMO_BUTTON) == LOW) { Serial.println("Demo button pressed"); wave(); delay(500); fingerTest(); delay(500); count(); delay(500); shaka(); delay(500); setDefaultPose(); delay(500); } } void rxHandler(BLEDevice central, BLECharacteristic characteristic) { String receivedData = reinterpret_cast(characteristic.value()); // get the data // Extract all data up to newline int newlinePos = receivedData.indexOf('\n'); if (newlinePos != -1) { receivedData = receivedData.substring(0, newlinePos); processCommand(receivedData); } } #define DOF_COUNT 17 void dofHandler(BLEDevice central, BLECharacteristic characteristic) { // The angles are packed into 8-bit values centered at 128, so we need to // unpack them and convert them back into decimal angles for the servos const uint8_t* data = characteristic.value(); // Simple data integrity checks if (characteristic.valueLength() != DOF_COUNT+1) { Serial.print("Invalid DOF length: "); Serial.println(characteristic.valueLength()); return; } // Last byte is a checksum - check against other bytes uint8_t checksum = 0; for (int i = 0; i < DOF_COUNT; i++) { checksum += data[i]; } if (checksum != data[DOF_COUNT]) { Serial.print("Invalid DOF checksum: "); Serial.println(checksum); return; } Serial.println(characteristic.valueLength()); int16_t unpackedAngles[DOF_COUNT]; for (int i = 0; i < DOF_COUNT; i++) { float angle = (data[i] - 127)*360.0/256.0; unpackedAngles[i] = static_cast(angle); } #ifdef DEBUG // Useful debug prints for tuning // Print out the angles Serial.print("DOF: "); for (int i = 0; i < DOF_COUNT; i++) { Serial.print(unpackedAngles[i]); Serial.print(" "); } Serial.println(); #endif // Fingers first for (int i = 0; i < NUM_FINGERS; i++) { fingers[i].setPitch(unpackedAngles[i*3]); fingers[i].setYaw(unpackedAngles[i*3+1]); fingers[i].setFlexion(unpackedAngles[i*3+2]); } // Thumb thumb.setPitch(unpackedAngles[12]); thumb.setYaw(unpackedAngles[13]); thumb.setFlexion(unpackedAngles[14]); // Wrist wrist.setPitch(unpackedAngles[15]); wrist.setYaw(unpackedAngles[16]); updateHand(); }