initial commit
This commit is contained in:
commit
1db19d110a
|
@ -0,0 +1,8 @@
|
||||||
|
{
|
||||||
|
// Use IntelliSense to learn about possible attributes.
|
||||||
|
// Hover to view descriptions of existing attributes.
|
||||||
|
"version": "0.2.0",
|
||||||
|
"configurations": [
|
||||||
|
|
||||||
|
]
|
||||||
|
}
|
|
@ -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();
|
||||||
|
|
||||||
|
}
|
|
@ -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));
|
||||||
|
|
||||||
|
}
|
|
@ -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
|
|
@ -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);
|
||||||
|
}
|
|
@ -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
|
|
@ -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
|
||||||
|
|
||||||
|
}
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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));
|
||||||
|
|
||||||
|
|
||||||
|
}
|
|
@ -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
|
|
@ -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());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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
|
Loading…
Reference in New Issue