167 lines
5.6 KiB
C++
167 lines
5.6 KiB
C++
|
#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));
|
||
|
|
||
|
|
||
|
}
|