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