|
@@ -44,6 +44,7 @@
|
44
|
44
|
#include "Configuration.h"
|
45
|
45
|
|
46
|
46
|
#ifdef NUM_SERVOS
|
|
47
|
+
|
47
|
48
|
#include <avr/interrupt.h>
|
48
|
49
|
#include <Arduino.h>
|
49
|
50
|
|
|
@@ -52,7 +53,6 @@
|
52
|
53
|
#define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
|
53
|
54
|
#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
|
54
|
55
|
|
55
|
|
-
|
56
|
56
|
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
|
57
|
57
|
|
58
|
58
|
//#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
|
|
@@ -74,24 +74,23 @@ uint8_t ServoCount = 0; // the total number
|
74
|
74
|
|
75
|
75
|
/************ static functions common to all instances ***********************/
|
76
|
76
|
|
77
|
|
-static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
|
78
|
|
-{
|
79
|
|
- if( Channel[timer] < 0 )
|
|
77
|
+static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) {
|
|
78
|
+ if (Channel[timer] < 0)
|
80
|
79
|
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
|
81
|
|
- else{
|
82
|
|
- if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
|
|
80
|
+ else {
|
|
81
|
+ if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive)
|
83
|
82
|
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
|
84
|
83
|
}
|
85
|
84
|
|
86
|
85
|
Channel[timer]++; // increment to the next channel
|
87
|
|
- if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
|
|
86
|
+ if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
|
88
|
87
|
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
|
89
|
|
- if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
|
|
88
|
+ if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
|
90
|
89
|
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
|
91
|
90
|
}
|
92
|
91
|
else {
|
93
|
92
|
// finished all channels so wait for the refresh period to expire before starting over
|
94
|
|
- if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
|
|
93
|
+ if ( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
|
95
|
94
|
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
|
96
|
95
|
else
|
97
|
96
|
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
|
|
@@ -100,142 +99,126 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
|
100
|
99
|
}
|
101
|
100
|
|
102
|
101
|
#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
|
103
|
|
-// Interrupt handlers for Arduino
|
104
|
|
-#if defined(_useTimer1)
|
105
|
|
-SIGNAL (TIMER1_COMPA_vect)
|
106
|
|
-{
|
107
|
|
- handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
108
|
|
-}
|
109
|
|
-#endif
|
110
|
|
-
|
111
|
|
-#if defined(_useTimer3)
|
112
|
|
-SIGNAL (TIMER3_COMPA_vect)
|
113
|
|
-{
|
114
|
|
- handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
115
|
|
-}
|
116
|
|
-#endif
|
117
|
102
|
|
118
|
|
-#if defined(_useTimer4)
|
119
|
|
-SIGNAL (TIMER4_COMPA_vect)
|
120
|
|
-{
|
121
|
|
- handle_interrupts(_timer4, &TCNT4, &OCR4A);
|
|
103
|
+ // Interrupt handlers for Arduino
|
|
104
|
+ #if defined(_useTimer1)
|
|
105
|
+ SIGNAL (TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
|
|
106
|
+ #endif
|
|
107
|
+
|
|
108
|
+ #if defined(_useTimer3)
|
|
109
|
+ SIGNAL (TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
|
|
110
|
+ #endif
|
|
111
|
+
|
|
112
|
+ #if defined(_useTimer4)
|
|
113
|
+ SIGNAL (TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); }
|
|
114
|
+ #endif
|
|
115
|
+
|
|
116
|
+ #if defined(_useTimer5)
|
|
117
|
+ SIGNAL (TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); }
|
|
118
|
+ #endif
|
|
119
|
+
|
|
120
|
+#else //!WIRING
|
|
121
|
+
|
|
122
|
+ // Interrupt handlers for Wiring
|
|
123
|
+ #if defined(_useTimer1)
|
|
124
|
+ void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
|
|
125
|
+ #endif
|
|
126
|
+ #if defined(_useTimer3)
|
|
127
|
+ void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
|
|
128
|
+ #endif
|
|
129
|
+
|
|
130
|
+#endif //!WIRING
|
|
131
|
+
|
|
132
|
+
|
|
133
|
+static void initISR(timer16_Sequence_t timer) {
|
|
134
|
+ #if defined(_useTimer1)
|
|
135
|
+ if (timer == _timer1) {
|
|
136
|
+ TCCR1A = 0; // normal counting mode
|
|
137
|
+ TCCR1B = _BV(CS11); // set prescaler of 8
|
|
138
|
+ TCNT1 = 0; // clear the timer count
|
|
139
|
+ #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
|
|
140
|
+ TIFR |= _BV(OCF1A); // clear any pending interrupts;
|
|
141
|
+ TIMSK |= _BV(OCIE1A); // enable the output compare interrupt
|
|
142
|
+ #else
|
|
143
|
+ // here if not ATmega8 or ATmega128
|
|
144
|
+ TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
|
|
145
|
+ TIMSK1 |= _BV(OCIE1A); // enable the output compare interrupt
|
|
146
|
+ #endif
|
|
147
|
+ #if defined(WIRING)
|
|
148
|
+ timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
|
|
149
|
+ #endif
|
|
150
|
+ }
|
|
151
|
+ #endif
|
|
152
|
+
|
|
153
|
+ #if defined(_useTimer3)
|
|
154
|
+ if (timer == _timer3) {
|
|
155
|
+ TCCR3A = 0; // normal counting mode
|
|
156
|
+ TCCR3B = _BV(CS31); // set prescaler of 8
|
|
157
|
+ TCNT3 = 0; // clear the timer count
|
|
158
|
+ #if defined(__AVR_ATmega128__)
|
|
159
|
+ TIFR |= _BV(OCF3A); // clear any pending interrupts;
|
|
160
|
+ ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
|
|
161
|
+ #else
|
|
162
|
+ TIFR3 = _BV(OCF3A); // clear any pending interrupts;
|
|
163
|
+ TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
|
|
164
|
+ #endif
|
|
165
|
+ #if defined(WIRING)
|
|
166
|
+ timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
|
|
167
|
+ #endif
|
|
168
|
+ }
|
|
169
|
+ #endif
|
|
170
|
+
|
|
171
|
+ #if defined(_useTimer4)
|
|
172
|
+ if (timer == _timer4) {
|
|
173
|
+ TCCR4A = 0; // normal counting mode
|
|
174
|
+ TCCR4B = _BV(CS41); // set prescaler of 8
|
|
175
|
+ TCNT4 = 0; // clear the timer count
|
|
176
|
+ TIFR4 = _BV(OCF4A); // clear any pending interrupts;
|
|
177
|
+ TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
|
|
178
|
+ }
|
|
179
|
+ #endif
|
|
180
|
+
|
|
181
|
+ #if defined(_useTimer5)
|
|
182
|
+ if (timer == _timer5) {
|
|
183
|
+ TCCR5A = 0; // normal counting mode
|
|
184
|
+ TCCR5B = _BV(CS51); // set prescaler of 8
|
|
185
|
+ TCNT5 = 0; // clear the timer count
|
|
186
|
+ TIFR5 = _BV(OCF5A); // clear any pending interrupts;
|
|
187
|
+ TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
|
|
188
|
+ }
|
|
189
|
+ #endif
|
122
|
190
|
}
|
123
|
|
-#endif
|
124
|
191
|
|
125
|
|
-#if defined(_useTimer5)
|
126
|
|
-SIGNAL (TIMER5_COMPA_vect)
|
127
|
|
-{
|
128
|
|
- handle_interrupts(_timer5, &TCNT5, &OCR5A);
|
|
192
|
+static void finISR(timer16_Sequence_t timer) {
|
|
193
|
+ // Disable use of the given timer
|
|
194
|
+ #if defined(WIRING)
|
|
195
|
+ if (timer == _timer1) {
|
|
196
|
+ #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
|
|
197
|
+ TIMSK1
|
|
198
|
+ #else
|
|
199
|
+ TIMSK
|
|
200
|
+ #endif
|
|
201
|
+ &= ~_BV(OCIE1A); // disable timer 1 output compare interrupt
|
|
202
|
+ timerDetach(TIMER1OUTCOMPAREA_INT);
|
|
203
|
+ }
|
|
204
|
+ else if (timer == _timer3) {
|
|
205
|
+ #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
|
|
206
|
+ TIMSK3
|
|
207
|
+ #else
|
|
208
|
+ ETIMSK
|
|
209
|
+ #endif
|
|
210
|
+ &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
|
211
|
+ timerDetach(TIMER3OUTCOMPAREA_INT);
|
|
212
|
+ }
|
|
213
|
+ #else //!WIRING
|
|
214
|
+ // For arduino - in future: call here to a currently undefined function to reset the timer
|
|
215
|
+ #endif
|
129
|
216
|
}
|
130
|
|
-#endif
|
131
|
217
|
|
132
|
|
-#elif defined WIRING
|
133
|
|
-// Interrupt handlers for Wiring
|
134
|
|
-#if defined(_useTimer1)
|
135
|
|
-void Timer1Service()
|
136
|
|
-{
|
137
|
|
- handle_interrupts(_timer1, &TCNT1, &OCR1A);
|
138
|
|
-}
|
139
|
|
-#endif
|
140
|
|
-#if defined(_useTimer3)
|
141
|
|
-void Timer3Service()
|
142
|
|
-{
|
143
|
|
- handle_interrupts(_timer3, &TCNT3, &OCR3A);
|
144
|
|
-}
|
145
|
|
-#endif
|
146
|
|
-#endif
|
147
|
|
-
|
148
|
|
-
|
149
|
|
-static void initISR(timer16_Sequence_t timer)
|
150
|
|
-{
|
151
|
|
-#if defined (_useTimer1)
|
152
|
|
- if(timer == _timer1) {
|
153
|
|
- TCCR1A = 0; // normal counting mode
|
154
|
|
- TCCR1B = _BV(CS11); // set prescaler of 8
|
155
|
|
- TCNT1 = 0; // clear the timer count
|
156
|
|
-#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
|
157
|
|
- TIFR |= _BV(OCF1A); // clear any pending interrupts;
|
158
|
|
- TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
|
159
|
|
-#else
|
160
|
|
- // here if not ATmega8 or ATmega128
|
161
|
|
- TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
|
162
|
|
- TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
|
163
|
|
-#endif
|
164
|
|
-#if defined(WIRING)
|
165
|
|
- timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
|
166
|
|
-#endif
|
167
|
|
- }
|
168
|
|
-#endif
|
169
|
|
-
|
170
|
|
-#if defined (_useTimer3)
|
171
|
|
- if(timer == _timer3) {
|
172
|
|
- TCCR3A = 0; // normal counting mode
|
173
|
|
- TCCR3B = _BV(CS31); // set prescaler of 8
|
174
|
|
- TCNT3 = 0; // clear the timer count
|
175
|
|
-#if defined(__AVR_ATmega128__)
|
176
|
|
- TIFR |= _BV(OCF3A); // clear any pending interrupts;
|
177
|
|
- ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
|
178
|
|
-#else
|
179
|
|
- TIFR3 = _BV(OCF3A); // clear any pending interrupts;
|
180
|
|
- TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
|
181
|
|
-#endif
|
182
|
|
-#if defined(WIRING)
|
183
|
|
- timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
|
184
|
|
-#endif
|
185
|
|
- }
|
186
|
|
-#endif
|
187
|
|
-
|
188
|
|
-#if defined (_useTimer4)
|
189
|
|
- if(timer == _timer4) {
|
190
|
|
- TCCR4A = 0; // normal counting mode
|
191
|
|
- TCCR4B = _BV(CS41); // set prescaler of 8
|
192
|
|
- TCNT4 = 0; // clear the timer count
|
193
|
|
- TIFR4 = _BV(OCF4A); // clear any pending interrupts;
|
194
|
|
- TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
|
195
|
|
- }
|
196
|
|
-#endif
|
197
|
|
-
|
198
|
|
-#if defined (_useTimer5)
|
199
|
|
- if(timer == _timer5) {
|
200
|
|
- TCCR5A = 0; // normal counting mode
|
201
|
|
- TCCR5B = _BV(CS51); // set prescaler of 8
|
202
|
|
- TCNT5 = 0; // clear the timer count
|
203
|
|
- TIFR5 = _BV(OCF5A); // clear any pending interrupts;
|
204
|
|
- TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
|
205
|
|
- }
|
206
|
|
-#endif
|
207
|
|
-}
|
208
|
|
-
|
209
|
|
-static void finISR(timer16_Sequence_t timer)
|
210
|
|
-{
|
211
|
|
- //disable use of the given timer
|
212
|
|
-#if defined WIRING // Wiring
|
213
|
|
- if(timer == _timer1) {
|
214
|
|
- #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
215
|
|
- TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
216
|
|
- #else
|
217
|
|
- TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
|
218
|
|
- #endif
|
219
|
|
- timerDetach(TIMER1OUTCOMPAREA_INT);
|
220
|
|
- }
|
221
|
|
- else if(timer == _timer3) {
|
222
|
|
- #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
|
223
|
|
- TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
224
|
|
- #else
|
225
|
|
- ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
|
226
|
|
- #endif
|
227
|
|
- timerDetach(TIMER3OUTCOMPAREA_INT);
|
228
|
|
- }
|
229
|
|
-#else
|
230
|
|
- //For arduino - in future: call here to a currently undefined function to reset the timer
|
231
|
|
-#endif
|
232
|
|
-}
|
233
|
|
-
|
234
|
|
-static boolean isTimerActive(timer16_Sequence_t timer)
|
235
|
|
-{
|
|
218
|
+static boolean isTimerActive(timer16_Sequence_t timer) {
|
236
|
219
|
// returns true if any servo is active on this timer
|
237
|
220
|
for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
|
238
|
|
- if(SERVO(timer,channel).Pin.isActive == true)
|
|
221
|
+ if (SERVO(timer,channel).Pin.isActive)
|
239
|
222
|
return true;
|
240
|
223
|
}
|
241
|
224
|
return false;
|
|
@@ -244,70 +227,59 @@ static boolean isTimerActive(timer16_Sequence_t timer)
|
244
|
227
|
|
245
|
228
|
/****************** end of static functions ******************************/
|
246
|
229
|
|
247
|
|
-Servo::Servo()
|
248
|
|
-{
|
249
|
|
- if( ServoCount < MAX_SERVOS) {
|
|
230
|
+Servo::Servo() {
|
|
231
|
+ if ( ServoCount < MAX_SERVOS) {
|
250
|
232
|
this->servoIndex = ServoCount++; // assign a servo index to this instance
|
251
|
|
- servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
|
|
233
|
+ servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
|
252
|
234
|
}
|
253
|
235
|
else
|
254
|
|
- this->servoIndex = INVALID_SERVO ; // too many servos
|
|
236
|
+ this->servoIndex = INVALID_SERVO; // too many servos
|
255
|
237
|
}
|
256
|
238
|
|
257
|
|
-uint8_t Servo::attach(int pin)
|
258
|
|
-{
|
|
239
|
+uint8_t Servo::attach(int pin) {
|
259
|
240
|
return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
|
260
|
241
|
}
|
261
|
242
|
|
262
|
|
-uint8_t Servo::attach(int pin, int min, int max)
|
263
|
|
-{
|
264
|
|
- if(this->servoIndex < MAX_SERVOS ) {
|
265
|
|
-#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
|
|
243
|
+uint8_t Servo::attach(int pin, int min, int max) {
|
|
244
|
+ if (this->servoIndex < MAX_SERVOS ) {
|
|
245
|
+ #if defined(ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
|
266
|
246
|
if (pin > 0) this->pin = pin; else pin = this->pin;
|
267
|
|
-#endif
|
268
|
|
- pinMode( pin, OUTPUT) ; // set servo pin to output
|
|
247
|
+ #endif
|
|
248
|
+ pinMode(pin, OUTPUT); // set servo pin to output
|
269
|
249
|
servos[this->servoIndex].Pin.nbr = pin;
|
270
|
250
|
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
271
|
|
- this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
|
272
|
|
- this->max = (MAX_PULSE_WIDTH - max)/4;
|
|
251
|
+ this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
|
|
252
|
+ this->max = (MAX_PULSE_WIDTH - max) / 4;
|
273
|
253
|
// initialize the timer if it has not already been initialized
|
274
|
254
|
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
275
|
|
- if(isTimerActive(timer) == false)
|
276
|
|
- initISR(timer);
|
|
255
|
+ if (!isTimerActive(timer)) initISR(timer);
|
277
|
256
|
servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
|
278
|
257
|
}
|
279
|
|
- return this->servoIndex ;
|
|
258
|
+ return this->servoIndex;
|
280
|
259
|
}
|
281
|
260
|
|
282
|
|
-void Servo::detach()
|
283
|
|
-{
|
|
261
|
+void Servo::detach() {
|
284
|
262
|
servos[this->servoIndex].Pin.isActive = false;
|
285
|
263
|
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
|
286
|
|
- if(isTimerActive(timer) == false) {
|
287
|
|
- finISR(timer);
|
288
|
|
- }
|
|
264
|
+ if (!isTimerActive(timer)) finISR(timer);
|
289
|
265
|
}
|
290
|
266
|
|
291
|
|
-void Servo::write(int value)
|
292
|
|
-{
|
293
|
|
- if(value < MIN_PULSE_WIDTH)
|
294
|
|
- { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
295
|
|
- if(value < 0) value = 0;
|
296
|
|
- if(value > 180) value = 180;
|
|
267
|
+void Servo::write(int value) {
|
|
268
|
+ if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
|
|
269
|
+ if (value < 0) value = 0;
|
|
270
|
+ if (value > 180) value = 180;
|
297
|
271
|
value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
|
298
|
272
|
}
|
299
|
273
|
this->writeMicroseconds(value);
|
300
|
274
|
}
|
301
|
275
|
|
302
|
|
-void Servo::writeMicroseconds(int value)
|
303
|
|
-{
|
|
276
|
+void Servo::writeMicroseconds(int value) {
|
304
|
277
|
// calculate and store the values for the given channel
|
305
|
278
|
byte channel = this->servoIndex;
|
306
|
|
- if( (channel < MAX_SERVOS) ) // ensure channel is valid
|
307
|
|
- {
|
308
|
|
- if( value < SERVO_MIN() ) // ensure pulse width is valid
|
|
279
|
+ if (channel < MAX_SERVOS) { // ensure channel is valid
|
|
280
|
+ if (value < SERVO_MIN()) // ensure pulse width is valid
|
309
|
281
|
value = SERVO_MIN();
|
310
|
|
- else if( value > SERVO_MAX() )
|
|
282
|
+ else if (value > SERVO_MAX())
|
311
|
283
|
value = SERVO_MAX();
|
312
|
284
|
|
313
|
285
|
value = value - TRIM_DURATION;
|
|
@@ -320,25 +292,13 @@ void Servo::writeMicroseconds(int value)
|
320
|
292
|
}
|
321
|
293
|
}
|
322
|
294
|
|
323
|
|
-int Servo::read() // return the value as degrees
|
324
|
|
-{
|
325
|
|
- return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
|
326
|
|
-}
|
|
295
|
+// return the value as degrees
|
|
296
|
+int Servo::read() { return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
|
327
|
297
|
|
328
|
|
-int Servo::readMicroseconds()
|
329
|
|
-{
|
330
|
|
- unsigned int pulsewidth;
|
331
|
|
- if( this->servoIndex != INVALID_SERVO )
|
332
|
|
- pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
|
333
|
|
- else
|
334
|
|
- pulsewidth = 0;
|
335
|
|
-
|
336
|
|
- return pulsewidth;
|
|
298
|
+int Servo::readMicroseconds() {
|
|
299
|
+ return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
|
337
|
300
|
}
|
338
|
301
|
|
339
|
|
-bool Servo::attached()
|
340
|
|
-{
|
341
|
|
- return servos[this->servoIndex].Pin.isActive ;
|
342
|
|
-}
|
|
302
|
+bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
|
343
|
303
|
|
344
|
304
|
#endif
|