@@ -69,13 +69,8 @@ uint8_t Servo::attach(int pin, uint16_t minUs, uint16_t maxUs)
6969uint8_t Servo::attach (int pin, uint16_t minUs, uint16_t maxUs, int value)
7070{
7171 if (!_attached) {
72- #ifdef WAVEFORM_LOCKED_PHASE
7372 pinMode (pin, OUTPUT);
7473 digitalWrite (pin, LOW);
75- #else
76- digitalWrite (pin, LOW);
77- pinMode (pin, OUTPUT);
78- #endif
7974 _pin = pin;
8075 _attached = true ;
8176 }
@@ -95,11 +90,7 @@ void Servo::detach()
9590{
9691 if (_attached) {
9792 _servoMap &= ~(1 << _pin);
98- #ifdef WAVEFORM_LOCKED_PHASE
9993 startWaveform (_pin, 0 , REFRESH_INTERVAL, 1 );
100- #else
101- // TODO - timeHigh == 0 is illegal in _PWM code branch. Do nothing for now.
102- #endif
10394 delay (REFRESH_INTERVAL / 1000 ); // long enough to complete active period under all circumstances.
10495 stopWaveform (_pin);
10596 _attached = false ;
@@ -124,13 +115,9 @@ void Servo::writeMicroseconds(int value)
124115 _valueUs = value;
125116 if (_attached) {
126117 _servoMap &= ~(1 << _pin);
127- #ifdef WAVEFORM_LOCKED_PHASE
128118 // Find the first GPIO being generated by checking GCC's find-first-set (returns 1 + the bit of the first 1 in an int32_t)
129119 int phaseReference = __builtin_ffs (_servoMap) - 1 ;
130120 if (startWaveform (_pin, _valueUs, REFRESH_INTERVAL - _valueUs, 0 , phaseReference))
131- #else
132- if (startWaveform (_pin, _valueUs, REFRESH_INTERVAL - _valueUs, 0 ))
133- #endif
134121 {
135122 _servoMap |= (1 << _pin);
136123 }
0 commit comments