The increase in baud rate is just to reduce the amount of time the debug printing spent in the interrupt handler.
Here is working code without the debug printing. The timings look ok for driving servos and are consistent, although the exact pulse widths are 25microseconds less than the calculated amount, that shouldn't be a problem in the real world.
Here is the latest code. It still needs testing and the overrun stuff.
Code:#define NBR_CHANNELS 4
#define DEFAULT_PULSE_WIDTH 1500
#define PULSING_COMPLETE (Channel >= NBR_CHANNELS) // true if all channels have been pulsed
byte servo = 3; // these could also be: #define servo 3
byte servoRoll = 5;
byte servoPitch = 6;
byte servoYaw = 11;
// arrays for channel info, note that the first channel is at index 0
byte counter[NBR_CHANNELS]; // holds pulse width / 128
byte remainder[NBR_CHANNELS]; // holds pulse width % 128
byte pins[NBR_CHANNELS] = {servo,servoRoll,servoPitch,servoYaw}; //holds pins associated with servos
volatile byte Channel = 0; // counter holding the channel being pulsed
volatile byte ISRCount; // counter used in the interrupt routines;
ISR (TIMER2_OVF_vect)
{
if (++ISRCount == counter[Channel] ) // are we on the final iteration for this channel
{
TCNT2 = 256 - remainder[Channel]; // yes, so count down the remainder
}
else if(ISRCount > counter[Channel])
{
// we have finished timing the channel so pulse it low and move on
digitalWrite(pins[Channel],LOW); // pulse this channel low
Channel++; // now increment to the next channel
ISRCount = 0; // reset the isr iteration counter
TCNT2 = 0;
if(Channel >= NBR_CHANNELS){ // check if we have done all channels
TIMSK2 = 0; // all channels pulsed, so disable interrupts
}
else
digitalWrite(pins[Channel],HIGH); // not done yet so pulse the next channel high
}
}
void SetPulse(byte channel, int pulsewidth){
// store the values for the given channel
counter[channel] = pulsewidth / 128;
remainder[channel] = pulsewidth % 128;
}
void PulseServos(){
// start the frame pulsing each servo in turn once
Channel = 0; // start from the first channel
ISRCount = 0; // reset the value of the ISR counter;
TCNT2 = 0;
TIFR2 = _BV(TOV2); // clear pending interrupts;
TIMSK2 = _BV(TOIE2) ; // enable the overflow interrupt
digitalWrite(pins[Channel],HIGH); // pulse the first channel high
}
void setup()
{
Serial.begin(38400); // note higher baud rate
for(int i=0; i < NBR_CHANNELS; i++) {
pinMode( pins[i], OUTPUT) ; // set servo pins to output
// SetPulse(i, DEFAULT_PULSE_WIDTH); // store default values for counter and compare
SetPulse(i, 1000 + (i*200)); // test pulses from 1ms to 1.6ms in 200us steps
}
pinMode(2, OUTPUT);
/* setup for timer 2 */
TIMSK2 = 0; // disable interrupts
TCCR2A = 0; // normal counting mode
TCCR2B = _BV(CS21); // set prescaler of 8
}
void loop()
{
//static unsigned long endtime;
unsigned long endtime = millis() + 20; //each frame is 20 milliseconds
PulseServos(); // start the interrupt handler pulsing the servos
delay( endtime - millis()); // wait until end of frame
// todo the above delays need enhancing to detect millis rollover
// note that it assumes that servos will have finished pulsing within 20ms
// There is a macro: PULSING_COMPLETE that evaluates to false if the ISR is still pulsing channels
}
Home
Pages: 
3 axis auto stabilized platform (Read 77668 times)
