From 1db19d110a3f194a44705104992076728051e5a0 Mon Sep 17 00:00:00 2001 From: allwin Date: Fri, 24 May 2024 16:36:04 +0200 Subject: [PATCH] initial commit --- .theia/launch.json | 8 + DexHand-RP2040-BLE.ino | 897 ++++++++++++++++++++++ Finger.cpp | 175 +++++ Finger.h | 94 +++ ManagedServo.cpp | 61 ++ ManagedServo.h | 68 ++ MathUtils.cpp | 19 + MathUtils.h | 17 + RP2040_ISR_Servo/RP2040_ISR_Servo.h | 32 + RP2040_ISR_Servo/RP2040_ISR_Servo.hpp | 275 +++++++ RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h | 71 ++ RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h | 425 ++++++++++ Thumb.cpp | 166 ++++ Thumb.h | 90 +++ Wrist.cpp | 77 ++ Wrist.h | 64 ++ 16 files changed, 2539 insertions(+) create mode 100644 .theia/launch.json create mode 100644 DexHand-RP2040-BLE.ino create mode 100644 Finger.cpp create mode 100644 Finger.h create mode 100644 ManagedServo.cpp create mode 100644 ManagedServo.h create mode 100644 MathUtils.cpp create mode 100644 MathUtils.h create mode 100644 RP2040_ISR_Servo/RP2040_ISR_Servo.h create mode 100644 RP2040_ISR_Servo/RP2040_ISR_Servo.hpp create mode 100644 RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h create mode 100644 RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h create mode 100644 Thumb.cpp create mode 100644 Thumb.h create mode 100644 Wrist.cpp create mode 100644 Wrist.h diff --git a/.theia/launch.json b/.theia/launch.json new file mode 100644 index 0000000..7e4253b --- /dev/null +++ b/.theia/launch.json @@ -0,0 +1,8 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [ + + ] +} diff --git a/DexHand-RP2040-BLE.ino b/DexHand-RP2040-BLE.ino new file mode 100644 index 0000000..457ce68 --- /dev/null +++ b/DexHand-RP2040-BLE.ino @@ -0,0 +1,897 @@ +#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(); + +} diff --git a/Finger.cpp b/Finger.cpp new file mode 100644 index 0000000..baf4c22 --- /dev/null +++ b/Finger.cpp @@ -0,0 +1,175 @@ +#include + +#include "ManagedServo.h" +#include "MathUtils.h" +#include "Finger.h" + + + + +Finger::Finger(String name, ManagedServo& leftPitchServo, ManagedServo& rightPitchServo, ManagedServo& flexionServo) +: mName(name), mLeftPitchServo(leftPitchServo), mRightPitchServo(rightPitchServo), mFlexionServo(flexionServo) { + // Default ranges to something sane, but they can be overriden by a tuning + // routine or by the user if desired. + mPitchRange[0] = 0; + mPitchRange[1] = 40; + mYawRange[0] = -20; + mYawRange[1] = 20; + mFlexionRange[0] = 0; + mFlexionRange[1] = 100; + mYawBias = 60; + + // Targets to nominal + mPitchTarget = 0; + mYawTarget = 0; + mFlexionTarget = 0; + +} + +Finger::~Finger() { +} + +// At the moment, there is no acceleration or deceleration of the servos or +// any other movement where there could be a delta between the target and actual +// position of the servos. However, we don't want to eliminate the possibility +// from future implementations, so the idea is that the servos will get a call +// to update() every loop, and this could be used for dynamic computation. +// +// Specifically, we are going to mix in the yaw angle with flexion. As the +// finger flexes, the yaw angle is reduced to keep the finger from bending +// sideways as the fingers align into more of a fist. + +void Finger::update() { + + // Update the pitch servos first + updatePitchServos(); + + // Scale the flexion angle to the range of the flexion servo + int32_t position = mapInteger(mFlexionTarget, mFlexionRange[0], mFlexionRange[1], + mFlexionServo.getMinPosition(), mFlexionServo.getMaxPosition()); + + #ifdef DEBUG // Useful debug printing for tuning + Serial.print("FlexTgt: "); + Serial.println(mFlexionTarget); + Serial.print("FlexRange:"); + Serial.print(mFlexionRange[0]); + Serial.print(" - "); + Serial.println(mFlexionRange[1]); + Serial.print("ServoRange:"); + Serial.print(mFlexionServo.getMinPosition()); + Serial.print(" - "); + Serial.println(mFlexionServo.getMaxPosition()); + Serial.print("FlexPos: "); + Serial.println(position); + #endif + + assert(position >= mFlexionServo.getMinPosition() && position <= mFlexionServo.getMaxPosition()); + mFlexionServo.setServoPosition(static_cast(position)); +} + + + +void Finger::setFlexion(int16_t flexion) { + mFlexionTarget = CLAMP(flexion, getFlexionMin(), getFlexionMax()); +} + +void Finger::setPitch(int16_t pitch) { + mPitchTarget = CLAMP(pitch, getPitchMin(), getPitchMax()); +} + +void Finger::setYaw(int16_t yaw) { + mYawTarget = CLAMP(yaw, getYawMin(), getYawMax()); +} + +void Finger::setMaxPosition() +{ + setPosition(getPitchMax(), 0, getFlexionMax()); +} + +void Finger::setMinPosition() +{ + setPosition(getPitchMin(), 0, getFlexionMin()); +} + +void Finger::setExtension(int16_t extension) +{ + // Clamp and invert + extension = 100-CLAMP(extension, 0, 100); + + // Map to the range of the pitch servos + int16_t pitch = mapInteger(extension, 0, 100, getPitchMin(), getPitchMax()); + setPitch(pitch); + + // Set the flexion + int16_t flexion = mapInteger(extension, 0, 100, getFlexionMin(), getFlexionMax()); + setFlexion(flexion); +} + + +// This is sort of the workhorse function - it does a couple of things: +// 1. It looks at the flexion angle and scales the yaw angle to keep the finger +// from bending sideways as the finger flexes, but also computes the delta +// between the two servos in order to try produce the requested yaw. +// 2. It then looks at the pitch angle and computes an overall position to +// send to the pitch servos to try produce the requested pitch. +// +// The yaw is computed and applied to a bias of the target positions on the left +// and right pitch servos. This pulls the finger to the left or right based +// on the value. Note - The bias value is kind of empiracal and something arrived +// at by tuning as opposed to a calculated value. This could potentially be calculated +// in a more accurate way down the road. + +void Finger::updatePitchServos() { + + float normalizedFlexion = normalizedValue(getFlexion(), mFlexionRange[0], mFlexionRange[1]); + + // Normalize the yaw + float normalizedYaw = normalizedValue(mYawTarget, mYawRange[0], mYawRange[1])-0.5f; + + // Scale the yaw based on the flexion angle, and apply the bias + float scaledYaw = normalizedYaw * (1.0f - normalizedFlexion) * mYawBias; + + #ifdef DEBUG // Useful debug printing for tuning + Serial.print("NFlex: "); + Serial.print(normalizedFlexion); + Serial.print(" YawTgt: "); + Serial.print(mYawTarget); + Serial.print(" NYaw: "); + Serial.print(normalizedYaw); + Serial.print("SYaw: "); + Serial.println(scaledYaw); + #endif + + // Compute the pitch on the servos + int32_t leftPitch = mapInteger(mPitchTarget, mPitchRange[0], mPitchRange[1], + mLeftPitchServo.getMinPosition(), mLeftPitchServo.getMaxPosition()); + + int32_t rightPitch = mapInteger(mPitchTarget, mPitchRange[0], mPitchRange[1], + mRightPitchServo.getMinPosition(), mRightPitchServo.getMaxPosition()); + + // Mix in the yaw + leftPitch = static_cast(leftPitch + scaledYaw); + rightPitch = static_cast(rightPitch - scaledYaw); + + // If flexion is > 50%, mix in additional pitch + if (normalizedFlexion > 0.5f) { + int flexionGain = static_cast((normalizedFlexion - 0.5f) * 30.0f); + + #ifdef DEBUG + Serial.print("FlexionGain: "); + Serial.println(flexionGain); + #endif + + leftPitch += flexionGain; + rightPitch += flexionGain; + } + + // Clamp + leftPitch = CLAMP(leftPitch, mLeftPitchServo.getMinPosition(), mLeftPitchServo.getMaxPosition()); + rightPitch = CLAMP(rightPitch, mRightPitchServo.getMinPosition(), mRightPitchServo.getMaxPosition()); + + // Send to servos + mLeftPitchServo.setServoPosition(static_cast(leftPitch)); + mRightPitchServo.setServoPosition(static_cast(rightPitch)); + +} diff --git a/Finger.h b/Finger.h new file mode 100644 index 0000000..8071ec6 --- /dev/null +++ b/Finger.h @@ -0,0 +1,94 @@ +#ifndef FINGER_H +#define FINGER_H + +/* +Finger Definition + +Finger is a class that represents a single finger on the DexHand. It is +responsible for managing the servos that control the finger's joints. + +Each finger consists of 3 servos. There are two differential servos +that control the knuckle pitch and yaw. The third servo controls the +finger's flexion which is the tendon running through the finger +to the tip. + +The finger class mixes together the signals for the two differential +servos to create the poses specified by the pitch and yaw angles from +the controller. + +Finger yaw is also modulated by the flexion angle. As the finger flexes, +the yaw angle is reduced to keep the finger from bending sideways as the +fingers align into more of a fist. +*/ +#include + +class ManagedServo; + +class Finger { + public: + Finger(String name, ManagedServo& leftPitchServo, ManagedServo& rightPitchServo, ManagedServo& flexionServo); + virtual ~Finger(); + + // Loop + void update(); // Called from the main loop to update the finger's servos + + // Positioning + inline void setPosition(int16_t pitch, int16_t yaw, int16_t flexion) { setPitch(pitch); setYaw(yaw); setFlexion(flexion);} + void setPitch(int16_t pitch); + void setYaw(int16_t yaw); + void setFlexion(int16_t flexion); + void setMaxPosition(); + void setMinPosition(); + void setExtension(int16_t percent); // Sets overall finger extension from 0 (closed) to 100 (open) + + inline String& getName() { return mName; } + + inline int16_t getPitch() const { return mPitchTarget;} + inline int16_t getYaw() const { return mYawTarget;} + inline int16_t getFlexion() const { return mFlexionTarget;} + + // Ranges + inline void setPitchRange(int16_t min, int16_t max) { mPitchRange[0] = min; mPitchRange[1] = max; } + inline void setYawRange(int16_t min, int16_t max) { mYawRange[0] = min; mYawRange[1] = max; } + inline void setYawBias(int16_t bias) { mYawBias = bias; } + inline void setFlexionRange(int16_t min, int16_t max) { mFlexionRange[0] = min; mFlexionRange[1] = max; } + + inline int16_t getPitchMin() const { return mPitchRange[0]; } + inline int16_t getPitchMax() const { return mPitchRange[1]; } + + inline int16_t getYawMin() const { return mYawRange[0]; } + inline int16_t getYawMax() const { return mYawRange[1]; } + inline int16_t getYawBias() const { return mYawBias; } + + inline int16_t getFlexionMin() const { return mFlexionRange[0]; } + inline int16_t getFlexionMax() const { return mFlexionRange[1]; } + + + + private: + String mName; + + ManagedServo& mLeftPitchServo; + ManagedServo& mRightPitchServo; + ManagedServo& mFlexionServo; + + int16_t mPitchTarget; + int16_t mYawTarget; + int16_t mFlexionTarget; + + int16_t mPitchRange[2]; + int16_t mYawRange[2]; + int16_t mFlexionRange[2]; + int16_t mYawBias; + + void updatePitchServos(); + + + + + + +}; + + +#endif \ No newline at end of file diff --git a/ManagedServo.cpp b/ManagedServo.cpp new file mode 100644 index 0000000..ef159a6 --- /dev/null +++ b/ManagedServo.cpp @@ -0,0 +1,61 @@ +#include "RP2040_ISR_Servo/RP2040_ISR_Servo.h" +#include "ManagedServo.h" + +// Published values for ES3352 servos; adjust if you are using different servos +#define MIN_MICROS 700 +#define MAX_MICROS 2300 +#define DEFAULT_MICROS MIN_MICROS+((MAX_MICROS-MIN_MICROS)/2) + + +ManagedServo::ManagedServo(uint8_t servoPin, uint8_t minPosition, uint8_t maxPosition, uint8_t defaultPosition, bool invertAngles) +: mServoPin(servoPin), mMinPosition(minPosition), mMaxPosition(maxPosition), + mDefaultPosition(defaultPosition), mCurrentPosition(defaultPosition), + mInvertAngles(invertAngles), mISRServoIndex(-1) { +} + +ManagedServo::~ManagedServo() { +} + +void ManagedServo::setupServo() +{ + mISRServoIndex = RP2040_ISR_Servos.setupServo(mServoPin, MIN_MICROS, MAX_MICROS); + + // Set default position + if (mISRServoIndex != -1) { + setServoPosition(mDefaultPosition); + } + else + { + Serial.print("Error setting up servo on pin "); + Serial.println(mServoPin); + } +} + +void ManagedServo::setServoPosition(uint8_t position) { + mCurrentPosition = position; + + if (mInvertAngles) { + position = 180 - position; + } + + if (position < mMinPosition) { + position = mMinPosition; + } else if (position > mMaxPosition) { + position = mMaxPosition; + } + + if (mISRServoIndex >= 0) { + RP2040_ISR_Servos.setPosition(mISRServoIndex, position); + } else { + Serial.print("Error setting servo position on pin "); + Serial.println(mServoPin); + } +} + +void ManagedServo::moveToMaxPosition() { + setServoPosition(mMaxPosition); +} + +void ManagedServo::moveToMinPosition() { + setServoPosition(mMinPosition); +} \ No newline at end of file diff --git a/ManagedServo.h b/ManagedServo.h new file mode 100644 index 0000000..67cca20 --- /dev/null +++ b/ManagedServo.h @@ -0,0 +1,68 @@ +#ifndef MANAGED_SERVO_H +#define MANAGED_SERVO_H + +#if ( defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_RASPBERRY_PI_PICO) || defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ + defined(ARDUINO_GENERIC_RP2040) ) && !defined(ARDUINO_ARCH_MBED) +#if !defined(RP2040_ISR_SERVO_USING_MBED) + #define RP2040_ISR_SERVO_USING_MBED false +#endif + +#elif ( defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_RASPBERRY_PI_PICO) || defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ + defined(ARDUINO_GENERIC_RP2040) ) && defined(ARDUINO_ARCH_MBED) + +#if !defined(RP2040_ISR_SERVO_USING_MBED) + #define RP2040_ISR_SERVO_USING_MBED true +#endif + +#else +#error This code is intended to run on the mbed / non-mbed RP2040 platform! Please check your Tools->Board setting. +#endif + +#define ISR_SERVO_DEBUG 0 + +#include "RP2040_ISR_Servo/RP2040_ISR_Servo.hpp" + + + +// The ManagedServo class is a wrapper around the Servo class that +// provides range checking and absolute limits on the servo position +// so that the model can't be asked to attain any position that would +// damage the model. + +class ManagedServo { + + public: + // Constructor + ManagedServo(uint8_t servoPin, uint8_t minPosition, uint8_t maxPosition, uint8_t defaultPosition, bool invertAngles); + virtual ~ManagedServo(); + + // Accessors + inline uint8_t getServoPin() const { return mServoPin; } + inline uint8_t getMinPosition() const { return mMinPosition; } + inline uint8_t getMaxPosition() const { return mMaxPosition; } + inline void setMinPosition(uint8_t minPosition) { if(minPosition > 0 && minPosition < 180) mMinPosition = minPosition; } + inline void setMaxPosition(uint8_t maxPosition) { if(maxPosition > 0 && maxPosition < 180) mMaxPosition = maxPosition; } + + inline uint8_t getDefaultPosition() { return mDefaultPosition; } + + // Initialization + void setupServo(); + + // Position control + void setServoPosition(uint8_t position); + inline uint8_t getServoPosition() const { return mCurrentPosition; } + void moveToMaxPosition(); + void moveToMinPosition(); + + private: + uint8_t mServoPin; + uint8_t mMinPosition; + uint8_t mMaxPosition; + uint8_t mDefaultPosition; + uint8_t mCurrentPosition; + bool mInvertAngles; + int mISRServoIndex; + +}; + +#endif \ No newline at end of file diff --git a/MathUtils.cpp b/MathUtils.cpp new file mode 100644 index 0000000..f1cc856 --- /dev/null +++ b/MathUtils.cpp @@ -0,0 +1,19 @@ +#include "MathUtils.h" + + +float normalizedValue(int32_t value, int32_t min, int32_t max) { + float scaled = static_cast(value - min) / static_cast(max - min); + return CLAMP(scaled, 0.0f, 1.0f); // Avoid epsilon errors +} + +int32_t mapInteger(int32_t value, int32_t inMin, int32_t inMax, int32_t outMin, int32_t outMax) { + // Get normalized value + float scaled = normalizedValue(value, inMin, inMax); + + // Scale to output range + int32_t val = static_cast(scaled * (outMax - outMin) + outMin); + + // Clamp + return CLAMP(val, outMin, outMax); // Avoid epsilon errors + +} diff --git a/MathUtils.h b/MathUtils.h new file mode 100644 index 0000000..c8069b1 --- /dev/null +++ b/MathUtils.h @@ -0,0 +1,17 @@ +#ifndef MATH_UTILS_H +#define MATH_UTILS_H + +#include + +// Clamp a value between a min and max +#define CLAMP(value, min, max) (value < min ? min : (value > max ? max : value)) + + +// Takes an integer and range, and returns a float between 0.0 and 1.0 +float normalizedValue(int32_t value, int32_t min, int32_t max); + +// Maps an integer value from one range to another +int32_t mapInteger(int32_t value, int32_t inMin, int32_t inMax, int32_t outMin, int32_t outMax); + + +#endif // MATH_UTILS_H \ No newline at end of file diff --git a/RP2040_ISR_Servo/RP2040_ISR_Servo.h b/RP2040_ISR_Servo/RP2040_ISR_Servo.h new file mode 100644 index 0000000..03a529a --- /dev/null +++ b/RP2040_ISR_Servo/RP2040_ISR_Servo.h @@ -0,0 +1,32 @@ +/**************************************************************************************************************************** + RP2040_ISR_Servo.h + For : + - MBED RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040. + - RP2040-based boards such as RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040 using arduino_pico core + + Written by Khoi Hoang + + Built by Khoi Hoang https://github.com/khoih-prog/RP2040_ISR_Servo + Licensed under MIT license + + Version: 1.1.2 + + Version Modified By Date Comments + ------- ----------- ---------- ----------- + 1.0.0 K Hoang 21/08/2021 Initial coding for RP2040 boards using ArduinoCore-mbed or arduino-pico core + 1.0.1 K Hoang 22/10/2021 Fix platform in library.json for PIO + 1.1.0 K Hoang 27/02/2022 Fix setPulseWidth() bug. Convert to h-only style + 1.1.1 K Hoang 08/03/2022 Delete redundant `.cpp` file causing compile error + 1.1.2 K Hoang 08/03/2022 Permit using servos with different pulse ranges simultaneously + *****************************************************************************************************************************/ + +#pragma once + +#ifndef RP2040_ISR_Servo_H +#define RP2040_ISR_Servo_H + +#include "RP2040_ISR_Servo.hpp" +#include "RP2040_ISR_Servo_Impl.h" + + +#endif diff --git a/RP2040_ISR_Servo/RP2040_ISR_Servo.hpp b/RP2040_ISR_Servo/RP2040_ISR_Servo.hpp new file mode 100644 index 0000000..52fffd0 --- /dev/null +++ b/RP2040_ISR_Servo/RP2040_ISR_Servo.hpp @@ -0,0 +1,275 @@ +/**************************************************************************************************************************** + RP2040_ISR_Servo.hpp + For : + - MBED RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040. + - RP2040-based boards such as RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040 using arduino_pico core + + Written by Khoi Hoang + Built by Khoi Hoang https://github.com/khoih-prog/RP2040_ISR_Servo + Licensed under MIT license + + Version: 1.1.2 + + Version Modified By Date Comments + ------- ----------- ---------- ----------- + 1.0.0 K Hoang 21/08/2021 Initial coding for RP2040 boards using ArduinoCore-mbed or arduino-pico core + 1.0.1 K Hoang 22/10/2021 Fix platform in library.json for PIO + 1.1.0 K Hoang 27/02/2022 Fix setPulseWidth() bug. Convert to h-only style + 1.1.1 K Hoang 08/03/2022 Delete redundant `.cpp` file causing compile error + 1.1.2 K Hoang 08/03/2022 Permit using servos with different pulse ranges simultaneously + *****************************************************************************************************************************/ + +#pragma once + +#ifndef RP2040_ISR_Servo_HPP +#define RP2040_ISR_Servo_HPP + +#if ( defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_RASPBERRY_PI_PICO) || defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ + defined(ARDUINO_GENERIC_RP2040) ) && !defined(ARDUINO_ARCH_MBED) + + #define RP2040_ISR_SERVO_VERSION "RP2040_ISR_Servo v1.1.2" + +#elif ( defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_RASPBERRY_PI_PICO) || defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ + defined(ARDUINO_GENERIC_RP2040) ) && defined(ARDUINO_ARCH_MBED) + + #define RP2040_ISR_SERVO_VERSION "Mbed RP2040_ISR_Servo v1.1.2" + +#else + #error This code is intended to run on the mbed / non-mbed RP2040 platform! Please check your Tools->Board setting. +#endif + +#define RP2040_ISR_SERVO_VERSION_MAJOR 1 +#define RP2040_ISR_SERVO_VERSION_MINOR 1 +#define RP2040_ISR_SERVO_VERSION_PATCH 2 + +#define RP2040_ISR_SERVO_VERSION_INT 1001002 + +#include + +#include + +#if defined(ARDUINO) + #if ARDUINO >= 100 + #include + #else + #include + #endif +#endif + +#include "RP2040_ISR_Servo_Debug.h" + +#define RP2040_MAX_PIN NUM_DIGITAL_PINS +#define RP2040_WRONG_PIN 255 + +// From Servo.h - Copyright (c) 2009 Michael Margolis. All right reserved. + +#define MIN_PULSE_WIDTH 800 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2450 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds + +///////////////////////////////////////////////////// + +#if defined(ARDUINO_ARCH_MBED) + +///////////////////////////////////////////////////// + +#include + +#if defined __has_include +# if __has_include ("pinDefinitions.h") +# include "pinDefinitions.h" +# endif +#endif + +///////////////////////////////////////////////////// + +class ServoImpl +{ + mbed::DigitalOut *pin; + mbed::Timeout timeout; // calls a callback once when a timeout expires + mbed::Ticker ticker; // calls a callback repeatedly with a timeout + + public: + + ServoImpl(const PinName& _pin) + { + pin = new mbed::DigitalOut(_pin); + } + + ~ServoImpl() + { + ticker.detach(); + timeout.detach(); + + delete pin; + } + + void start(const uint32_t& duration_us) + { + duration = duration_us; + + // in microsecs + ticker.attach(mbed::callback(this, &ServoImpl::call), (std::chrono::microseconds) 20000); + } + + void call() + { + timeout.attach(mbed::callback(this, &ServoImpl::toggle), (std::chrono::microseconds) duration); + toggle(); + } + + void toggle() + { + *pin = !*pin; + } + + int32_t duration = -1; +}; + +///////////////////////////////////////////////////// + +#else + +///////////////////////////////////////////////////// + +#include + +///////////////////////////////////////////////////// + +#endif + +///////////////////////////////////////////////////// + + // For mbed core +class RP2040_ISR_Servo +{ + + public: + // maximum number of servos + const static int MAX_SERVOS = 18; + + // constructor + RP2040_ISR_Servo(); + + // destructor + ~RP2040_ISR_Servo() + { + } + + // Bind servo to the timer and pin, return servoIndex + int8_t setupServo(const uint8_t& pin, const uint16_t& minPulseUs = MIN_PULSE_WIDTH, const uint16_t& maxPulseUs = MAX_PULSE_WIDTH, uint16_t value = 0); + + // if value is < MIN_PULSE_WIDTH its treated as an angle, otherwise as pulse width in microseconds + void write(const uint8_t& servoIndex, uint16_t& value); + + // Write pulse width in microseconds + void writeMicroseconds(const uint8_t& servoIndex, uint16_t value); + + // setPosition will set servo to position in degrees + // by using PWM, turn HIGH 'duration' microseconds within REFRESH_INTERVAL (20000us) + // returns true on success or -1 on wrong servoIndex + bool setPosition(const uint8_t& servoIndex, uint16_t position); + + // returns last position in degrees if success, or -1 on wrong servoIndex + int getPosition(const uint8_t& servoIndex); + + // setPulseWidth will set servo PWM Pulse Width in microseconds, correcponding to certain position in degrees + // by using PWM, turn HIGH 'pulseWidth' microseconds within REFRESH_INTERVAL (20000us) + // min and max for each individual servo are enforced + // returns true on success or -1 on wrong servoIndex + bool setPulseWidth(const uint8_t& servoIndex, uint16_t& pulseWidth); + + // returns pulseWidth in microsecs (within min/max range) if success, or 0 on wrong servoIndex + uint16_t getPulseWidth(const uint8_t& servoIndex); + + // destroy the specified servo + void deleteServo(const uint8_t& servoIndex); + + // returns true if the specified servo is enabled + bool isEnabled(const uint8_t& servoIndex); + + // enables the specified servo + bool enable(const uint8_t& servoIndex); + + // disables the specified servo + bool disable(const uint8_t& servoIndex); + + // enables all servos + void enableAll(); + + // disables all servos + void disableAll(); + + // enables the specified servo if it's currently disabled, + // and vice-versa + bool toggle(const uint8_t& servoIndex); + + // returns the number of used servos + int8_t getNumServos(); + + // returns the number of available servos + int8_t getNumAvailableServos() + { + if (numServos <= 0) + return MAX_SERVOS; + else + return MAX_SERVOS - numServos; + }; + + private: + + void init() + { + for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) + { + memset((void*) &servo[servoIndex], 0, sizeof (servo_t)); + servo[servoIndex].enabled = false; + // Intentional bad pin + servo[servoIndex].pin = RP2040_WRONG_PIN; + } + + numServos = 0; + } + + // find the first available slot + int8_t findFirstFreeSlot(); + +#if defined(ARDUINO_ARCH_MBED) + + typedef struct + { + uint8_t pin; // pin servo connected to + int position; // In degrees + bool enabled; // true if enabled + uint16_t minPulseUs; // The minimum pulse width the servo can handle + uint16_t maxPulseUs; // The maximum pulse width the servo can handle + ServoImpl* servoImpl; + } servo_t; + +#else + + typedef struct + { + uint8_t pin; // pin servo connected to + PIO pio; + int smIdx; // The state machine inside + int pgmOffset; // Where in the SM the code lives + int position; // In degrees + bool enabled; // true if enabled + uint16_t minPulseUs; // The minimum pulse width the servo can handle + uint16_t maxPulseUs; // The maximum pulse width the servo can handle + } servo_t; + +#endif + + servo_t servo[MAX_SERVOS]; + + // actual number of servos in use (-1 means uninitialized) + int8_t numServos; +}; + +///////////////////////////////////////////////////// + + +#endif // #ifndef RP2040_ISR_Servo_HPP diff --git a/RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h b/RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h new file mode 100644 index 0000000..2594485 --- /dev/null +++ b/RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h @@ -0,0 +1,71 @@ +/**************************************************************************************************************************** + RP2040_ISR_Servo_Debug.h + For : + - MBED RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040. + - RP2040-based boards such as RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040 using arduino_pico core + + Written by Khoi Hoang + + Built by Khoi Hoang https://github.com/khoih-prog/RP2040_ISR_Servo + Licensed under MIT license + + Version: 1.1.2 + + Version Modified By Date Comments + ------- ----------- ---------- ----------- + 1.0.0 K Hoang 21/08/2021 Initial coding for RP2040 boards using ArduinoCore-mbed or arduino-pico core + 1.0.1 K Hoang 22/10/2021 Fix platform in library.json for PIO + 1.1.0 K Hoang 27/02/2022 Fix setPulseWidth() bug. Convert to h-only style + 1.1.1 K Hoang 08/03/2022 Delete redundant `.cpp` file causing compile error + 1.1.2 K Hoang 08/03/2022 Permit using servos with different pulse ranges simultaneously + *****************************************************************************************************************************/ + +#pragma once + +#ifndef RP2040_ISR_Servo_Debug_h +#define RP2040_ISR_Servo_Debug_h + +////////////////////////////////////////// + +#ifndef ISR_SERVO_DEBUG + #define ISR_SERVO_DEBUG 1 +#endif + +////////////////////////////////////////// + +#if !defined(ISR_SERVO_DEBUG_OUTPUT) + #define ISR_SERVO_DEBUG_OUTPUT Serial +#endif + +////////////////////////////////////////////////////// + +const char ISR_SERVO_MARK[] = "[ISR_SERVO] "; +const char ISR_SERVO_SP[] = " "; + +#define ISR_SERVO_PRINT ISR_SERVO_DEBUG_OUTPUT.print +#define ISR_SERVO_PRINTLN ISR_SERVO_DEBUG_OUTPUT.println +#define ISR_SERVO_FLUSH ISR_SERVO_DEBUG_OUTPUT.flush + +#define ISR_SERVO_PRINT_MARK ISR_SERVO_PRINT(ISR_SERVO_MARK) +#define ISR_SERVO_PRINT_SP ISR_SERVO_PRINT(ISR_SERVO_SP) + +////////////////////////////////////////////////////// + +#define ISR_SERVO_LOGERROR(x) if(ISR_SERVO_DEBUG>0) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINTLN(x); } +#define ISR_SERVO_LOGERROR0(x) if(ISR_SERVO_DEBUG>0) { ISR_SERVO_PRINT(x); } +#define ISR_SERVO_LOGERROR1(x,y) if(ISR_SERVO_DEBUG>0) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(y); } +#define ISR_SERVO_LOGERROR2(x,y,z) if(ISR_SERVO_DEBUG>0) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(y); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(z); } +#define ISR_SERVO_LOGERROR3(x,y,z,w) if(ISR_SERVO_DEBUG>0) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(y); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(z); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(w); } + + +////////////////////////////////////////////////////// + +#define ISR_SERVO_LOGDEBUG(x) if(ISR_SERVO_DEBUG>1) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINTLN(x); } +#define ISR_SERVO_LOGDEBUG0(x) if(ISR_SERVO_DEBUG>1) { ISR_SERVO_PRINT(x); } +#define ISR_SERVO_LOGDEBUG1(x,y) if(ISR_SERVO_DEBUG>1) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(y); } +#define ISR_SERVO_LOGDEBUG2(x,y,z) if(ISR_SERVO_DEBUG>1) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(y); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(z); } +#define ISR_SERVO_LOGDEBUG3(x,y,z,w) if(ISR_SERVO_DEBUG>1) { ISR_SERVO_PRINT_MARK; ISR_SERVO_PRINT(x); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(y); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINT(z); ISR_SERVO_PRINT_SP; ISR_SERVO_PRINTLN(w); } +////////////////////////////////////////// + + +#endif // RP2040_ISR_Servo_Debug_h diff --git a/RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h b/RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h new file mode 100644 index 0000000..905fbef --- /dev/null +++ b/RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h @@ -0,0 +1,425 @@ +/**************************************************************************************************************************** + RP2040_ISR_Servo_Impl_H.h + For : + - MBED RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040. + - RP2040-based boards such as RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040 using arduino_pico core + + Written by Khoi Hoang + Built by Khoi Hoang https://github.com/khoih-prog/RP2040_ISR_Servo + Licensed under MIT license + + Version: 1.1.2 + + Version Modified By Date Comments + ------- ----------- ---------- ----------- + 1.0.0 K Hoang 21/08/2021 Initial coding for RP2040 boards using ArduinoCore-mbed or arduino-pico core + 1.0.1 K Hoang 22/10/2021 Fix platform in library.json for PIO + 1.1.0 K Hoang 27/02/2022 Fix setPulseWidth() bug. Convert to h-only style + 1.1.1 K Hoang 08/03/2022 Delete redundant `.cpp` file causing compile error + 1.1.2 K Hoang 08/03/2022 Permit using servos with different pulse ranges simultaneously + *****************************************************************************************************************************/ + +#pragma once + +#ifndef RP2040_ISR_Servo_Impl_H +#define RP2040_ISR_Servo_Impl_H + +#include "RP2040_ISR_Servo.h" +#include + +#ifndef ISR_SERVO_DEBUG + #define ISR_SERVO_DEBUG 1 +#endif + +static RP2040_ISR_Servo RP2040_ISR_Servos; // create servo object to control up to 16 servos + +#if defined(ARDUINO_ARCH_MBED) + + #define TRIM_DURATION 15 //callback overhead (35 us) -> 15 us if toggle() is called after starting the timeout + +#else + + #include "servo.pio.h" + static PIOProgram _servoPgm(&servo_program); + +#endif + +///////////////////////////////////////////////////// + +RP2040_ISR_Servo::RP2040_ISR_Servo() + : numServos (-1) +{ +} + +///////////////////////////////////////////////////// + +// find the first available slot +// return -1 if none found +int8_t RP2040_ISR_Servo::findFirstFreeSlot() +{ + // all slots are used + if (numServos >= MAX_SERVOS) + return -1; + + // return the first slot with no count (i.e. free) + for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) + { + if (servo[servoIndex].enabled == false) + { + ISR_SERVO_LOGDEBUG1("Index =", servoIndex); + + return servoIndex; + } + } + + // no free slots found + return -1; +} + +///////////////////////////////////////////////////// + +int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minPulseUs, const uint16_t& maxPulseUs, + uint16_t value) +{ + int8_t servoIndex; + + if (pin > RP2040_MAX_PIN) + return -1; + + pinMode(pin, OUTPUT); + digitalWrite(pin, LOW); + + if (numServos < 0) + init(); + + servoIndex = findFirstFreeSlot(); + + if (servoIndex < 0) + return -1; + + servo[servoIndex].pin = pin; + servo[servoIndex].maxPulseUs = maxPulseUs; + servo[servoIndex].minPulseUs = minPulseUs; + servo[servoIndex].position = 0; + servo[servoIndex].enabled = true; + + numServos++; + +#if defined(ARDUINO_ARCH_MBED) + + // Add code here for mbed + servo[servoIndex].servoImpl = new ServoImpl(digitalPinToPinName(pin)); + +#else + + if (!_servoPgm.prepare(&servo[servoIndex].pio, &servo[servoIndex].smIdx, &servo[servoIndex].pgmOffset)) + { + // ERROR, no free slots + ISR_SERVO_LOGERROR("Error no free slot"); + return -1; + } + + servo[servoIndex].enabled = true; + + servo_program_init(servo[servoIndex].pio, servo[servoIndex].smIdx, servo[servoIndex].pgmOffset, pin); + pio_sm_set_enabled(servo[servoIndex].pio, servo[servoIndex].smIdx, false); + pio_sm_put_blocking(servo[servoIndex].pio, servo[servoIndex].smIdx, RP2040::usToPIOCycles(REFRESH_INTERVAL) / 3); + pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_pull(false, false)); + pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_out(pio_isr, 32)); + + write(servoIndex, value); + + pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_pull(false, false)); + pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_mov(pio_x, pio_osr)); + pio_sm_set_enabled(servo[servoIndex].pio, servo[servoIndex].smIdx, true); + + ///////////////////////////////////////// + + write(servoIndex, value); + +#endif + + ISR_SERVO_LOGDEBUG1("Index =", servoIndex); + ISR_SERVO_LOGDEBUG3("min =", servo[servoIndex].minPulseUs, ", max =", servo[servoIndex].maxPulseUs); + + return servoIndex; +} + +///////////////////////////////////////////////////// + +void RP2040_ISR_Servo::write(const uint8_t& servoIndex, uint16_t& value) +{ + // treat any value less than MIN_PULSE_WIDTH as angle in degrees (values equal or larger are handled as microseconds) + if (value < MIN_PULSE_WIDTH) + { + // assumed to be 0-180 degrees servo + value = constrain(value, 0, 180); + value = map(value, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); + } + + writeMicroseconds(servoIndex, value); +} + +///////////////////////////////////////////////////// + +void RP2040_ISR_Servo::writeMicroseconds(const uint8_t& servoIndex, uint16_t value) +{ + value = constrain(value, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); + servo[servoIndex].position = value; + + if (servo[servoIndex].enabled) + { +#if defined(ARDUINO_ARCH_MBED) + + value = value - TRIM_DURATION; + + if (servo[servoIndex].servoImpl->duration == -1) + { + servo[servoIndex].servoImpl->start(value); + } + + servo[servoIndex].servoImpl->duration = value; +#else + + // Remove any old updates that haven't yet taken effect + pio_sm_clear_fifos(servo[servoIndex].pio, servo[servoIndex].smIdx); + pio_sm_put_blocking(servo[servoIndex].pio, servo[servoIndex].smIdx, RP2040::usToPIOCycles(value) / 3); + +#endif + } +} + +///////////////////////////////////////////////////// + +bool RP2040_ISR_Servo::setPosition(const uint8_t& servoIndex, uint16_t position) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + // Updates interval of existing specified servo + if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) + { + // treat any value less than MIN_PULSE_WIDTH as angle in degrees (values equal or larger are handled as microseconds) + if (position < MIN_PULSE_WIDTH) + { + // assumed to be 0-180 degrees servo + position = constrain(position, 0, 180); + position = map(position, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); + } + + servo[servoIndex].position = position; + + writeMicroseconds(servoIndex, position); + + ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position); + + return true; + } + + // false return for non-used numServo or bad pin + return false; +} + +///////////////////////////////////////////////////// + +// returns last position in degrees if success, or -1 on wrong servoIndex +int RP2040_ISR_Servo::getPosition(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return -1; + + // Updates interval of existing specified servo + if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) + { + ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position); + + return (servo[servoIndex].position); + } + + // return 0 for non-used numServo or bad pin + return -1; +} + +///////////////////////////////////////////////////// + +// setPulseWidth will set servo PWM Pulse Width in microseconds, correcponding to certain position in degrees +// by using PWM, turn HIGH 'pulseWidth' microseconds within REFRESH_INTERVAL (20000us) +// min and max for each individual servo are enforced +// returns true on success or -1 on wrong servoIndex +bool RP2040_ISR_Servo::setPulseWidth(const uint8_t& servoIndex, uint16_t& pulseWidth) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + // Updates interval of existing specified servo + if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) + { + if (pulseWidth < servo[servoIndex].minPulseUs) + pulseWidth = servo[servoIndex].minPulseUs; + else if (pulseWidth > servo[servoIndex].maxPulseUs) + pulseWidth = servo[servoIndex].maxPulseUs; + + writeMicroseconds(servoIndex, pulseWidth); + + ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position); + + + return true; + } + + // false return for non-used numServo or bad pin + return false; +} + +///////////////////////////////////////////////////// + +// returns pulseWidth in microsecs (within min/max range) if success, or 0 on wrong servoIndex +uint16_t RP2040_ISR_Servo::getPulseWidth(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return 0; + + // Updates interval of existing specified servo + if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) + { + ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position); + + return (servo[servoIndex].position); + } + + // return 0 for non-used numServo or bad pin + return 0; +} + +///////////////////////////////////////////////////// + +void RP2040_ISR_Servo::deleteServo(const uint8_t& servoIndex) +{ + if ( (numServos == 0) || (servoIndex >= MAX_SERVOS) ) + { + return; + } + + // don't decrease the number of servos if the specified slot is already empty + if (servo[servoIndex].enabled) + { +#if defined(ARDUINO_ARCH_MBED) + + //Must be before memset + if (servo[servoIndex].servoImpl) + delete servo[servoIndex].servoImpl; + +#endif + + memset((void*) &servo[servoIndex], 0, sizeof (servo_t)); + + servo[servoIndex].enabled = false; + servo[servoIndex].position = 0; + // Intentional bad pin, good only from 0-16 for Digital, A0=17 + servo[servoIndex].pin = RP2040_WRONG_PIN; + + // update number of servos + numServos--; + } +} + +///////////////////////////////////////////////////// + +bool RP2040_ISR_Servo::isEnabled(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + if (servo[servoIndex].pin > RP2040_MAX_PIN) + { + // Disable if something wrong + servo[servoIndex].pin = RP2040_WRONG_PIN; + servo[servoIndex].enabled = false; + return false; + } + + return servo[servoIndex].enabled; +} + +///////////////////////////////////////////////////// + +bool RP2040_ISR_Servo::enable(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + if (servo[servoIndex].pin > RP2040_MAX_PIN) + { + // Disable if something wrong + servo[servoIndex].pin = RP2040_WRONG_PIN; + servo[servoIndex].enabled = false; + return false; + } + + if ( servo[servoIndex].position >= servo[servoIndex].minPulseUs ) + servo[servoIndex].enabled = true; + + return true; +} + +///////////////////////////////////////////////////// + +bool RP2040_ISR_Servo::disable(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + if (servo[servoIndex].pin > RP2040_MAX_PIN) + servo[servoIndex].pin = RP2040_WRONG_PIN; + + servo[servoIndex].enabled = false; + + return true; +} + +void RP2040_ISR_Servo::enableAll() +{ + // Enable all servos with a enabled and count != 0 (has PWM) and good pin + for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) + { + if ( (servo[servoIndex].position >= servo[servoIndex].minPulseUs) && !servo[servoIndex].enabled + && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) + { + servo[servoIndex].enabled = true; + } + } +} + +///////////////////////////////////////////////////// + +void RP2040_ISR_Servo::disableAll() +{ + // Disable all servos + for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) + { + servo[servoIndex].enabled = false; + } +} + +///////////////////////////////////////////////////// + +bool RP2040_ISR_Servo::toggle(const uint8_t& servoIndex) +{ + if (servoIndex >= MAX_SERVOS) + return false; + + servo[servoIndex].enabled = !servo[servoIndex].enabled; + + return true; +} + +///////////////////////////////////////////////////// + +int8_t RP2040_ISR_Servo::getNumServos() +{ + return numServos; +} + +///////////////////////////////////////////////////// + +#endif // RP2040_ISR_Servo_Impl_H diff --git a/Thumb.cpp b/Thumb.cpp new file mode 100644 index 0000000..951ebeb --- /dev/null +++ b/Thumb.cpp @@ -0,0 +1,166 @@ +#include + +#include "ManagedServo.h" +#include "MathUtils.h" +#include "Thumb.h" + + + + +Thumb::Thumb(ManagedServo& leftPitchServo, ManagedServo& rightPitchServo, ManagedServo& flexionServo, ManagedServo& rollServo) +: mLeftPitchServo(leftPitchServo), mRightPitchServo(rightPitchServo), mFlexionServo(flexionServo), mRollServo(rollServo) { + // Default ranges to something sane, but they can be overriden by a tuning + // routine or by the user if desired. + mPitchRange[0] = 30; + mPitchRange[1] = 60; + mYawRange[0] = 0; + mYawRange[1] = 45; + mFlexionRange[0] = 0; + mFlexionRange[1] = 45; + mRollRange[0] = 0; + mRollRange[1] = 20; + + // Targets to nominal + mPitchTarget = 0; + mYawTarget = 0; + mFlexionTarget = 0; + mRollTarget = 0; + +} + +Thumb::~Thumb() { +} + +// At the moment, there is no acceleration or deceleration of the servos or +// any other movement where there could be a delta between the target and actual +// position of the servos. However, we don't want to eliminate the possibility +// from future implementations, so the idea is that the servos will get a call +// to update() every loop, and this could be used for dynamic computation. +// +// Specifically, we are going to mix in the yaw angle and roll angle as a function +// of pitch and yaw. This is an approximation and not a perfect science, but it +// produces a reasonable aesthetic result for the thumb. + +void Thumb::update() { + + // Update the pitch servos first + updatePitchServos(); + + // Scale the flexion angle to the range of the flexion servo + int32_t position = mapInteger(mFlexionTarget, mFlexionRange[0], mFlexionRange[1], + mFlexionServo.getMinPosition(), mFlexionServo.getMaxPosition()); + + assert(position >= mFlexionServo.getMinPosition() && position <= mFlexionServo.getMaxPosition()); + mFlexionServo.setServoPosition(static_cast(position)); + + // Scale the roll angle to the range of the roll servo + position = mapInteger(mRollTarget, mRollRange[0], mRollRange[1], + mRollServo.getMinPosition(), mRollServo.getMaxPosition()); + + assert(position >= mRollServo.getMinPosition() && position <= mRollServo.getMaxPosition()); + mRollServo.setServoPosition(static_cast(position)); +} + + + +void Thumb::setFlexion(int16_t flexion) { + mFlexionTarget = CLAMP(flexion, getFlexionMin(), getFlexionMax()); +} + +void Thumb::setPitch(int16_t pitch) { + mPitchTarget = CLAMP(pitch, getPitchMin(), getPitchMax()); +} + +void Thumb::setYaw(int16_t yaw) { + mYawTarget = CLAMP(yaw, getYawMin(), getYawMax()); +} + +void Thumb::setRoll(int16_t roll) { + mRollTarget = CLAMP(roll, getRollMin(), getRollMax()); +} + +void Thumb::setMaxPosition() +{ + setPosition(getPitchMax(), getYawMax(), getFlexionMax()); + setRoll(getRollMax()); + update(); +} + +void Thumb::setMinPosition() +{ + setPosition(getPitchMin(), getYawMin(), getFlexionMin()); + setRoll(getRollMin()); + update(); +} + +void Thumb::setExtension(int16_t extension) +{ + // Clamp and invert + extension = 100-CLAMP(extension, 0, 100); + + // Map to the range of the pitch servos + int16_t pitch = mapInteger(extension, 0, 100, getPitchMin(), getPitchMax()); + setPitch(pitch); + + // Set the flexion + int16_t flexion = mapInteger(extension, 0, 100, getFlexionMin(), getFlexionMax()); + setFlexion(flexion); + + // Set the roll + int16_t roll = mapInteger(extension, 0, 100, getRollMin(), getRollMax()); + setRoll(roll); +} + + +/* This function is a little bit tricky and worthy of explanation: + + The Yaw angle coming through from the MediaPipe tracker has a range of approximately 45 degrees. + At approximately 30 degrees, the thumb is roughly parallel with the index finger. + Beyond 30 degrees, it actually crosses over to the palm. So, we approximate this motion + by adding the angle to the upper thumb servo (right pitch servo) up to 30 degrees and then + subtract it off quickly beyond that to allow the thumb to extend back out past the fingers. + + This is not really a "mathematically correct" solution per se, but gives the intended output + without very difficult math, and it approximates the fidelity of the data that MediaPipe Hand + Tracker returns at an appropriate level on the hardware for now. +*/ + +#define YAW_THRESHOLD 30 +void Thumb::updatePitchServos() { + + // If thumb is in yaw range before crossing over the palm, perform regular calculation and apply to right servo + if (mYawTarget < YAW_THRESHOLD) { + int32_t clamped = CLAMP(mYawTarget, mYawRange[0], YAW_THRESHOLD); + int32_t rightPitch = mapInteger(clamped, mYawRange[0], YAW_THRESHOLD, + mRightPitchServo.getMinPosition(), mRightPitchServo.getMaxPosition()); + + mRightPitchServo.setServoPosition(static_cast(rightPitch)); + } + else { + // Thumb is crossing over face of palm - subtract off 2X the overage amount so that + // we mix the right pitch servo angle out while increasing the left servo. + int32_t yawOver = mYawTarget-YAW_THRESHOLD; + + // Subtract overage from right servo + int32_t clamped = CLAMP(YAW_THRESHOLD-2*yawOver, mYawRange[0], YAW_THRESHOLD); + int32_t rightPitch = mapInteger(clamped, mYawRange[0], YAW_THRESHOLD, + mRightPitchServo.getMinPosition(), mRightPitchServo.getMaxPosition()); + + mRightPitchServo.setServoPosition(static_cast(rightPitch)); + + #ifdef DEBUG + Serial.print("Yaw over:"); + Serial.print(yawOver); + Serial.print(" Adj rt pitch:"); + Serial.print(clamped); + #endif + } + + // Apply pitch to left servo + int32_t leftPitch = mapInteger(mPitchTarget, mPitchRange[0], mPitchRange[1], + mLeftPitchServo.getMinPosition(), mLeftPitchServo.getMaxPosition()); + + mLeftPitchServo.setServoPosition(static_cast(leftPitch)); + + +} diff --git a/Thumb.h b/Thumb.h new file mode 100644 index 0000000..f592f27 --- /dev/null +++ b/Thumb.h @@ -0,0 +1,90 @@ +#ifndef THUMB_H +#define THUMB_H + +/* +Thumb Definition + +Thumb is a class that represents the thumb on the DexHand. It is +responsible for managing the servos that control the thumb joints. + +Each thumb consists of 4 servos. There are two differential servos +that control the knuckle pitch and yaw. The third servo controls the +thumb's flexion which is the tendon running through the finger +to the tip. And the fourth controls the roll of the thumb. + +The thumb class mixes together the signals for the two differential +servos to create the poses specified by the pitch and yaw angles from +the controller. + +Roll is also automatically computed based on the pitch and yaw angles. + +*/ +#include + +class ManagedServo; + +class Thumb { + public: + Thumb(ManagedServo& leftPitchServo, ManagedServo& rightPitchServo, ManagedServo& flexionServo, ManagedServo& rollServo); + virtual ~Thumb(); + + // Loop + void update(); // Called from the main loop to update the thumb servos + + // Positioning + inline void setPosition(int16_t pitch, int16_t yaw, int16_t flexion) { setPitch(pitch); setYaw(yaw); setFlexion(flexion);} + void setPitch(int16_t pitch); + void setYaw(int16_t yaw); + void setFlexion(int16_t flexion); + void setRoll(int16_t roll); + void setMaxPosition(); + void setMinPosition(); + void setExtension(int16_t percent); // Sets overall thumb extension from 0 (closed) to 100 (open) + + + inline int16_t getPitch() const { return mPitchTarget;} + inline int16_t getYaw() const { return mYawTarget;} + inline int16_t getFlexion() const { return mFlexionTarget;} + + // Ranges + inline void setPitchRange(int16_t min, int16_t max) { mPitchRange[0] = min; mPitchRange[1] = max; } + inline void setYawRange(int16_t min, int16_t max) { mYawRange[0] = min; mYawRange[1] = max; } + inline void setFlexionRange(int16_t min, int16_t max) { mFlexionRange[0] = min; mFlexionRange[1] = max; } + inline void setRollRange(int16_t min, int16_t max) { mRollRange[0] = min; mRollRange[1] = max; } + + inline int16_t getPitchMin() const { return mPitchRange[0]; } + inline int16_t getPitchMax() const { return mPitchRange[1]; } + + inline int16_t getYawMin() const { return mYawRange[0]; } + inline int16_t getYawMax() const { return mYawRange[1]; } + + inline int16_t getFlexionMin() const { return mFlexionRange[0]; } + inline int16_t getFlexionMax() const { return mFlexionRange[1]; } + + inline int16_t getRollMin() const { return mRollRange[0]; } + inline int16_t getRollMax() const { return mRollRange[1]; } + + + + + private: + ManagedServo& mLeftPitchServo; + ManagedServo& mRightPitchServo; + ManagedServo& mFlexionServo; + ManagedServo& mRollServo; + + int16_t mPitchTarget; + int16_t mYawTarget; + int16_t mFlexionTarget; + int16_t mRollTarget; + + int16_t mPitchRange[2]; + int16_t mYawRange[2]; + int16_t mFlexionRange[2]; + int16_t mRollRange[2]; + + void updatePitchServos(); +}; + + +#endif \ No newline at end of file diff --git a/Wrist.cpp b/Wrist.cpp new file mode 100644 index 0000000..4d28680 --- /dev/null +++ b/Wrist.cpp @@ -0,0 +1,77 @@ +#include + +#include "ManagedServo.h" +#include "MathUtils.h" +#include "Wrist.h" + + + + +Wrist::Wrist(ManagedServo& leftPitchServo, ManagedServo& rightPitchServo) +: mLeftPitchServo(leftPitchServo), mRightPitchServo(rightPitchServo) { + // Default ranges to something sane, but they can be overriden by a tuning + // routine or by the user if desired. + mPitchRange[0] = -40; + mPitchRange[1] = 40; + mYawRange[0] = -40; + mYawRange[1] = 40; + + // Targets to nominal + mPitchTarget = 0; + mYawTarget = 0; + +} + +Wrist::~Wrist() { +} + +/* The wrist angles are created by two differential servos. Combinations of moving these + servos with, and against each other create the range of motion on the wrist. + + To help visualize this, you can use this table to understand how the range of motion is + attained. + + Left Pitch Right Pitch Result + + MIN MIN Hand centered (pitch) and to the left (yaw) + MAX MAX Hand centered (pitch) and to the right (yaw) + + MIN MAX Hand to the back (pitch) and centered(yaw) + MAX MIN Hand to the front (pitch) and centered (yaw) + + With the orientation of servos in the hand and windings of the cables, the equations + below are used to calculate the angles of the servos to achieve the desired wrist + position. + + Left Pitch Servo = pitch + yaw + Right Pitch Servo = yaw - pitch + +*/ +void Wrist::update() { + + int32_t leftCumulative = mPitchTarget + mYawTarget; + int32_t rightCumulative = mYawTarget - mPitchTarget; + + int32_t leftPos = mapInteger(leftCumulative, mPitchRange[0], mPitchRange[1], + mLeftPitchServo.getMinPosition(), mLeftPitchServo.getMaxPosition()); + int32_t rightPos = mapInteger(rightCumulative, mPitchRange[0], mPitchRange[1], + mRightPitchServo.getMinPosition(), mRightPitchServo.getMaxPosition()); + + mLeftPitchServo.setServoPosition(static_cast(leftPos)); + mRightPitchServo.setServoPosition(static_cast(rightPos)); +} + + + +void Wrist::setPitch(int16_t pitch) { + mPitchTarget = CLAMP(pitch, getPitchMin(), getPitchMax()); + + +} + +void Wrist::setYaw(int16_t yaw) { + mYawTarget = CLAMP(yaw, getYawMin(), getYawMax()); +} + + + diff --git a/Wrist.h b/Wrist.h new file mode 100644 index 0000000..87650ac --- /dev/null +++ b/Wrist.h @@ -0,0 +1,64 @@ +#ifndef WRIST_H +#define WRIST_H + +/* +Wrist Definition + +Wrist is a class that represents the wrist on the DexHand. It is +responsible for managing the servos that control the wrist joints. + +Each wrust consists of 2 differential servos that control the +wrist pitch and yaw. + +The wrist class mixes together the signals for the two differential +servos to create the poses specified by the pitch and yaw angles from +the controller. +*/ + +#include + +class ManagedServo; + +class Wrist { + public: + Wrist(ManagedServo& leftPitchServo, ManagedServo& rightPitchServo); + virtual ~Wrist(); + + // Loop + void update(); // Called from the main loop to update the wrist servos + + // Positioning + inline void setPosition(int16_t pitch, int16_t yaw) { setPitch(pitch); setYaw(yaw); } + void setPitch(int16_t pitch); + void setYaw(int16_t yaw); + inline void setDefaultPosition() { setPitch(0); setYaw(0); } + + inline int16_t getPitch() const { return mPitchTarget;} + inline int16_t getYaw() const { return mYawTarget;} + + // Ranges + inline void setPitchRange(int16_t min, int16_t max) { mPitchRange[0] = min; mPitchRange[1] = max; } + inline void setYawRange(int16_t min, int16_t max) { mYawRange[0] = min; mYawRange[1] = max; } + + inline int16_t getPitchMin() const { return mPitchRange[0]; } + inline int16_t getPitchMax() const { return mPitchRange[1]; } + + inline int16_t getYawMin() const { return mYawRange[0]; } + inline int16_t getYawMax() const { return mYawRange[1]; } + + + + private: + ManagedServo& mLeftPitchServo; + ManagedServo& mRightPitchServo; + + int16_t mPitchTarget; + int16_t mYawTarget; + + int16_t mPitchRange[2]; + int16_t mYawRange[2]; + +}; + + +#endif \ No newline at end of file