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