mirror of
https://github.com/digistump/DigistumpArduino.git
synced 2025-04-28 07:39:02 -07:00
152 lines
3.7 KiB
C++
152 lines
3.7 KiB
C++
/*
|
|
Copyright (c) 2013 Benjamin Holt
|
|
|
|
The MIT License (MIT) - See README for complete license
|
|
*/
|
|
|
|
|
|
#include <SimpleServo.h>
|
|
|
|
|
|
///// Setup /////
|
|
SimpleServo::SimpleServo() :
|
|
_pin(0xff),
|
|
_degrees(0xff),
|
|
_microseconds(0),
|
|
_maxDegrees(180),
|
|
_millisPer60degrees(230), // default from http://www.servodatabase.com/servo/futaba/s3003
|
|
_minWriteMillis(0),
|
|
_maxWriteMillis(0xffff),
|
|
_minPulse(700),
|
|
_maxPulse(2300),
|
|
_pulseMillis(16)
|
|
{}
|
|
|
|
|
|
uint8_t SimpleServo::attach(uint8_t p) {
|
|
_pin = p;
|
|
_degrees = 0xff;
|
|
_microseconds = 0;
|
|
// Deliberately not (re)initializing servo parameters
|
|
pinMode(_pin, OUTPUT);
|
|
digitalWrite(_pin, LOW);
|
|
return 1;
|
|
}
|
|
|
|
|
|
void SimpleServo::detach() {
|
|
_pin = 0xff;
|
|
}
|
|
|
|
|
|
uint8_t SimpleServo::attached() {
|
|
return _pin != 0xff;
|
|
}
|
|
/////
|
|
|
|
|
|
///// Movement /////
|
|
void SimpleServo::_pulse(uint16_t ms) {
|
|
// This actually moves the servo
|
|
uint16_t elapsedMillis = 0;
|
|
while (elapsedMillis < ms) {
|
|
digitalWrite(_pin,HIGH);
|
|
delayMicroseconds(_microseconds);
|
|
digitalWrite(_pin,LOW);
|
|
if (ms > 0) { // 0ms: do the pulse, but that's it (probably a bad idea but, hey, if it works with your project, cool)
|
|
delay(_pulseMillis); // ENHANCEME: A hook to allow the client to make use (at their own risk of adding jitter) of these millis would be really nice
|
|
}
|
|
elapsedMillis += _pulseMillis + _microseconds / 1000 + 1;
|
|
}
|
|
}
|
|
|
|
|
|
void SimpleServo::writeMicrosecondsMillis(uint16_t us, uint16_t ms) {
|
|
_microseconds = us; // Allow unconstrained us
|
|
if (_minPulse <= us && us <= _maxPulse) {
|
|
_degrees = map(us, _minPulse, _maxPulse, 0, _maxDegrees);
|
|
} else {
|
|
_degrees = 0xff; // Out-of-range us do not convert sensibly to degrees
|
|
}
|
|
_pulse(ms); // And unconstrained ms, don't point this method at your foot.
|
|
}
|
|
|
|
|
|
void SimpleServo::writeMillis(uint8_t deg, uint16_t ms) {
|
|
_degrees = constrain(deg, 0, _maxDegrees);
|
|
_microseconds = constrain(map(_degrees, 0, _maxDegrees, _minPulse, _maxPulse), _minPulse, _maxPulse);
|
|
_pulse(ms); // Allow unconstrained millis
|
|
}
|
|
|
|
|
|
uint32_t SimpleServo::millisToTarget(uint8_t deg) {
|
|
uint8_t delta = abs(_degrees - deg); // if degrees is 0xff, this will over-estimate, so don't do anything special in that case
|
|
return ((uint32_t)_millisPer60degrees * delta) / 60;
|
|
}
|
|
|
|
|
|
void SimpleServo::writeMicroseconds(uint16_t us) {
|
|
// Simplest to do the us -> deg conversion here, so just finish the job
|
|
_microseconds = constrain(us, 0, _maxPulse); // keep degrees sane as much as possible, you have writeMicrosecondsMillis if you need to bend the rules
|
|
uint8_t target = map(us, _minPulse, _maxPulse, 0, _maxDegrees);
|
|
uint32_t millis = millisToTarget(target);
|
|
millis = constrain(millis, _minWriteMillis, _maxWriteMillis);
|
|
_degrees = target;
|
|
_pulse((uint16_t)millis);
|
|
}
|
|
|
|
|
|
void SimpleServo::write(uint8_t deg) {
|
|
uint32_t millis = millisToTarget(deg);
|
|
millis = constrain(millis, _minWriteMillis, _maxWriteMillis);
|
|
writeMillis(deg, (uint16_t)millis);
|
|
}
|
|
|
|
|
|
uint8_t SimpleServo::read() {
|
|
return _degrees;
|
|
}
|
|
|
|
|
|
uint16_t SimpleServo::readMicroseconds() {
|
|
return _microseconds;
|
|
}
|
|
/////
|
|
|
|
|
|
///// Servo Parameters /////
|
|
void SimpleServo::setMaximumDegrees(uint8_t deg) {
|
|
_maxDegrees = deg;
|
|
}
|
|
|
|
|
|
void SimpleServo::setMillisPer60Degrees(uint16_t ms) {
|
|
_millisPer60degrees = ms;
|
|
}
|
|
|
|
|
|
void SimpleServo::setMinimumMillis(uint16_t ms) {
|
|
_minWriteMillis = ms;
|
|
}
|
|
|
|
|
|
void SimpleServo::setMaximumMillis(uint16_t ms) {
|
|
_maxWriteMillis = ms;
|
|
}
|
|
|
|
|
|
void SimpleServo::setMinimumPulse(uint16_t us) {
|
|
_minPulse = us;
|
|
}
|
|
|
|
|
|
void SimpleServo::setMaximumPulse(uint16_t us) {
|
|
_maxPulse = us;
|
|
}
|
|
|
|
|
|
void SimpleServo::setPulseMillis(uint8_t ms) {
|
|
_pulseMillis = ms;
|
|
}
|
|
/////
|