allwin.lucious
/
testrepo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial commit

This commit is contained in:
allwin 2024-05-24 16:36:04 +02:00
commit 1db19d110a
16 changed files with 2539 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.theia/launch.json Normal file
View File

@ -0,0 +1,8 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
"version": "0.2.0",
"configurations": [
]
}

897
DexHand-RP2040-BLE.ino Normal file
View File

@ -0,0 +1,897 @@
#include <ArduinoBLE.h>
#include <UniversalTimer.h>
#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<const char*>(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<int16_t>(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();
}

175
Finger.cpp Normal file
View File

@ -0,0 +1,175 @@
#include <assert.h>
#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<uint8_t>(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<int32_t>(leftPitch + scaledYaw);
rightPitch = static_cast<int32_t>(rightPitch - scaledYaw);
// If flexion is > 50%, mix in additional pitch
if (normalizedFlexion > 0.5f) {
int flexionGain = static_cast<int32_t>((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<uint8_t>(leftPitch));
mRightPitchServo.setServoPosition(static_cast<uint8_t>(rightPitch));
}

94
Finger.h Normal file
View File

@ -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 <Arduino.h>
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

61
ManagedServo.cpp Normal file
View File

@ -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);
}

68
ManagedServo.h Normal file
View File

@ -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

19
MathUtils.cpp Normal file
View File

@ -0,0 +1,19 @@
#include "MathUtils.h"
float normalizedValue(int32_t value, int32_t min, int32_t max) {
float scaled = static_cast<float>(value - min) / static_cast<float>(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<int32_t>(scaled * (outMax - outMin) + outMin);
// Clamp
return CLAMP(val, outMin, outMax); // Avoid epsilon errors
}

17
MathUtils.h Normal file
View File

@ -0,0 +1,17 @@
#ifndef MATH_UTILS_H
#define MATH_UTILS_H
#include <Arduino.h>
// 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

32
RP2040_ISR_Servo/RP2040_ISR_Servo.h Normal file
View File

@ -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

275
RP2040_ISR_Servo/RP2040_ISR_Servo.hpp Normal file
View File

@ -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 <stddef.h>
#include <inttypes.h>
#if defined(ARDUINO)
#if ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#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 <mbed.h>
#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 <hardware/pio.h>
/////////////////////////////////////////////////////
#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

71
RP2040_ISR_Servo/RP2040_ISR_Servo_Debug.h Normal file
View File

@ -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

425
RP2040_ISR_Servo/RP2040_ISR_Servo_Impl.h Normal file
View File

@ -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 <string.h>
#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

166
Thumb.cpp Normal file
View File

@ -0,0 +1,166 @@
#include <assert.h>
#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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(leftPitch));
}

90
Thumb.h Normal file
View File

@ -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 <Arduino.h>
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

77
Wrist.cpp Normal file
View File

@ -0,0 +1,77 @@
#include <assert.h>
#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<uint8_t>(leftPos));
mRightPitchServo.setServoPosition(static_cast<uint8_t>(rightPos));
}
void Wrist::setPitch(int16_t pitch) {
mPitchTarget = CLAMP(pitch, getPitchMin(), getPitchMax());
}
void Wrist::setYaw(int16_t yaw) {
mYawTarget = CLAMP(yaw, getYawMin(), getYawMax());
}

64
Wrist.h Normal file
View File

@ -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 <Arduino.h>
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