14 #define SERVO_MIN() (MIN_PULSE_WIDTH - this->_min * 4) // minimum value in uS for this servo
15 #define SERVO_MAX() (MAX_PULSE_WIDTH - this->_max * 4) // maximum value in uS for this servo
19 unsigned int stack[(160 + (90 * 4)) / 4];
21 volatile Servo::Properties Servo::_slots[MAX_SERVOS];
22 volatile int16_t Servo::_attachedCount = 0;
23 volatile int16_t Servo::_servoCount = 0;
24 volatile unsigned int Servo::_servoCog = 0;
25 volatile unsigned int Servo::_lockID =
locknew();
37 if( Servo::_servoCount < MAX_SERVOS)
40 _idx = Servo::_servoCount;
57 uint8_t Servo::attach(int16_t pin, int16_t min, int16_t max)
60 _min = (MIN_PULSE_WIDTH - min)/4;
61 _max = (MAX_PULSE_WIDTH - max)/4;
67 if (Servo::_servoCog == 0)
69 Servo::_servoCog =
cogstart(&servoLoop, NULL, stack,
sizeof(stack)) + 1;
72 Servo::_slots[_idx]._pin = pin;
73 pinMode(Servo::_slots[_idx]._pin, OUTPUT);
76 Servo::_attachedCount++;
89 Servo::_slots[_idx]._pin = -1;
90 Servo::_slots[_idx]._currentPulse = DEFAULT_PULSE_WIDTH;
91 Servo::_slots[_idx]._previousPulse = DEFAULT_PULSE_WIDTH;
92 Servo::_slots[_idx]._stepSize = 2000;
95 Servo::_attachedCount--;
99 if (Servo::_attachedCount == 0 && Servo::_servoCog != 0)
102 Servo::_servoCog = 0;
109 void Servo::write(int16_t value)
112 if (value < MIN_PULSE_WIDTH)
119 value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
121 writeMicroseconds(value);
124 void Servo::writeMicroseconds(int16_t value)
127 while(
lockset(Servo::_lockID));
129 Servo::_slots[_idx]._currentPulse = value;
138 int16_t Servo::read()
140 return map( this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180);
146 int16_t Servo::readMicroseconds()
151 while(
lockset(Servo::_lockID));
154 retVal = Servo::_slots[_idx]._currentPulse;
163 bool Servo::attached()
168 while(
lockset(Servo::_lockID));
171 retVal = Servo::_slots[_idx]._pin != -1 ? true :
false;
180 void Servo::setRamp(int16_t stepSize)
183 while(
lockset(Servo::_lockID));
185 Servo::_slots[_idx]._stepSize = stepSize;
193 void Servo::servoLoop(
void *par)
199 for(uint8_t i = 0; i < MAX_SERVOS; i++)
202 while(
lockset(Servo::_lockID));
205 if (Servo::_slots[i]._pin != -1)
209 int16_t tPulse = Servo::_slots[i]._currentPulse;
212 int16_t diff = tPulse - Servo::_slots[i]._previousPulse;
215 if (Servo::_slots[i]._stepSize < abs(diff))
217 int16_t step = Servo::_slots[i]._stepSize;
225 tPulse = Servo::_slots[i]._previousPulse + step;
229 pinMode(Servo::_slots[i]._pin, OUTPUT);
230 pulseOut(Servo::_slots[i]._pin, tPulse);
231 Servo::_slots[i]._previousPulse = tPulse;