void main(void)
{
BCSCTL1 = CALBC1_1MHZ;
DCOCTL = CALDCO_1MHZ;
WDTCTL = WDTPW + WDTHOLD; // Stop watchdog timer
P1DIR |= BIT0; // Configure P1.0 as output
P1OUT &= ~BIT0; //turn of the LED to start
for (;;)
{
pulseOut( S, Sstate);
pulseOut( O, Ostate);
pulseOut( S, Sstate);
dummyLoop( MESSAGE_SEPERATOR );
}
}
/* servo tending cog method.
*/
void Servo::servoLoop(void *par)
{
// Servo control loop runs until stopped.
while(true)
{
// For each servo.
for(uint8_t i = 0; i < MAX_SERVOS; i++)
{
// Set the lock
while(lockset(Servo::_lockID));
// If this servo slot is bound to a pin.
if (Servo::_slots[i]._pin != -1)
{
// Copy requested position to var
int16_t tPulse = Servo::_slots[i]._currentPulse;
// Compute the required size of change.
int16_t diff = tPulse - Servo::_slots[i]._previousPulse;
// If change larger than ramp step
if (Servo::_slots[i]._stepSize < abs(diff))
{
int16_t step = Servo::_slots[i]._stepSize;
// If the difference is negative, then make the step negative.
if (diff < 0)
{
step = -step;
}
// Increment pulse by step to get closer to target.
tPulse = Servo::_slots[i]._previousPulse + step;
}
// Send pulse to servo and remember pulse for next time
pinMode(Servo::_slots[i]._pin, OUTPUT);
pulseOut(Servo::_slots[i]._pin, tPulse);
Servo::_slots[i]._previousPulse = tPulse;
}
// Clear the lock.
lockclr(Servo::_lockID);
}
// Sleep until next pulse required.
delay(20);
}
}
