|
@ -52,14 +52,14 @@ uint8_t ServoCount; //=0 |
|
|
#define TAU_USEC (TAU_MSEC * 1000) |
|
|
#define TAU_USEC (TAU_MSEC * 1000) |
|
|
#define TAU_CYC (TAU_MSEC * CYC_MSEC) |
|
|
#define TAU_CYC (TAU_MSEC * CYC_MSEC) |
|
|
#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) |
|
|
#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) |
|
|
#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER)) |
|
|
#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER)) |
|
|
|
|
|
|
|
|
// Unit conversions
|
|
|
// Unit conversions
|
|
|
#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) |
|
|
#define US_TO_COMPARE(us) ((uint16_t)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) |
|
|
#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) |
|
|
#define COMPARE_TO_US(c) ((uint32_t)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) |
|
|
#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \ |
|
|
#define ANGLE_TO_US(a) ((uint16_t)(map((a), this->minAngle, this->maxAngle, \ |
|
|
SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW))) |
|
|
SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW))) |
|
|
#define US_TO_ANGLE(us) ((int16)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \ |
|
|
#define US_TO_ANGLE(us) ((int16_t)(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, \ |
|
|
this->minAngle, this->maxAngle))) |
|
|
this->minAngle, this->maxAngle))) |
|
|
|
|
|
|
|
|
libServo::libServo() { |
|
|
libServo::libServo() { |
|
@ -74,7 +74,7 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m |
|
|
this->maxAngle = maxAngle; |
|
|
this->maxAngle = maxAngle; |
|
|
|
|
|
|
|
|
timer_dev *tdev = PIN_MAP[this->pin].timer_device; |
|
|
timer_dev *tdev = PIN_MAP[this->pin].timer_device; |
|
|
uint8 tchan = PIN_MAP[this->pin].timer_channel; |
|
|
uint8_t tchan = PIN_MAP[this->pin].timer_channel; |
|
|
|
|
|
|
|
|
pinMode(this->pin, PWM); |
|
|
pinMode(this->pin, PWM); |
|
|
pwmWrite(this->pin, 0); |
|
|
pwmWrite(this->pin, 0); |
|
@ -97,7 +97,7 @@ bool libServo::detach() { |
|
|
int32_t libServo::read() const { |
|
|
int32_t libServo::read() const { |
|
|
if (this->attached()) { |
|
|
if (this->attached()) { |
|
|
timer_dev *tdev = PIN_MAP[this->pin].timer_device; |
|
|
timer_dev *tdev = PIN_MAP[this->pin].timer_device; |
|
|
uint8 tchan = PIN_MAP[this->pin].timer_channel; |
|
|
uint8_t tchan = PIN_MAP[this->pin].timer_channel; |
|
|
return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan))); |
|
|
return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan))); |
|
|
} |
|
|
} |
|
|
return 0; |
|
|
return 0; |
|
|