Go Down

Topic: 3 axis auto stabilized platform (Read 77563 times) previous topic - next topic

mem

Quote
hi.. i tried the usual way of digitally writing the servos... i found that the problem is when the program is looping... sometimes there's a gap which has no pulses

i think that's the reason why its resulting in the servos reacting this way...

i'm thinking whether is it because of the IF loop?

do you mean this code?
Code: [Select]

 if ((PulseWidth <= 2200) && (PulseWidth >= 600))   // 600 is a little low, try it with 800
 {
   digitalWrite(pin_num, HIGH);  
   delayMicroseconds(PulseWidth);
   digitalWrite(pin_num, LOW);
 }
 else {
        ; you may want to add code here to blink a LED if Pulse WIdth is out of range
 }

If so, did you add the code to see if you are sending values that are out of range? It will help if you can determine if your problem is the calculation of the servo pulse width or the servo driving code.

Also, if you post the entire sketch you are testing then there may be some tips from people here to help you further.

axileon

i checked the pulsewidth... its alright... everything is within range...

arlight.. here's the code..

Code: [Select]
#define icpPin            8   // this interrupt handler must use pin 8
#define TICKS_PER_uS      2    // number of timer ticks per microsecond
#define MAX_CHANNELS    3         // maximum number of channels we want  
volatile unsigned int Pulses[ MAX_CHANNELS + 1]; // array holding channel pulses width value in microseconds
volatile uint8_t Channel;      // number of channels detected so far in the frame (first channel is 1)

boolean DataAvailable; // set to true when we have received data for all channels
boolean Ready;         // true when we are ready to detect the leading edge of a channel

int RefreshTime = 2;
int lastPulse;

int a = 13;
int b = 12;
int c = 11;
int g = 6;

// here is the logic of how this code works :
// to start, channel is set to 1 and ready to false and the DataAvailable flag is false
// when a negative edge is detected ready is set to true
// when a positive edge is detected and ready is true then the measurement starts
// when a negative edge is detected and the ready flag is true then the measurement ends, the ready flag is reset back to false
//  and the channel is incremented. If the channel count after incrementing is greater than the number of channels then
//  the DataAvailable flag is set true and pulse data can be accessed

ISR(TIMER1_CAPT_vect){
  if( !bit_is_set(TCCR1B ,ICES1)){       // was falling edge detected ?  
        if(Ready) {
             Pulses[Channel] = ICR1 / TICKS_PER_uS;  // store pulse length as microsoeconds
           
                 if(++Channel > MAX_CHANNELS)
           {
                   Channel = 1;       //reset the channel counter to 1      
                 DataAvailable = true;  // this line is needed to indicate that the channel data is valid!!!!
               }
                SelectChannel( Channel);
             // Add code here to gate the multiplexer to the current channel          
        }          
        Ready = !Ready;  //toggle the ready flag
  }
  else {                       // rising  edge was detected  
       TCNT1 = 0;               // reset the counter      
                         
  }    
  TCCR1B ^= _BV(ICES1);                 // toggle bit value to trigger on the other edge    
}

void setup()                    // run once, when the sketch starts
{
 Serial.begin(9600);  
 
 pinMode(icpPin,INPUT);
   
 pinMode(g,OUTPUT);
 pinMode(a,OUTPUT);
 pinMode(b,OUTPUT);
 pinMode(c,OUTPUT);
 pinMode(2,OUTPUT);
 pinMode(3,OUTPUT);
 pinMode(4,OUTPUT);
 pinMode(7,OUTPUT);
 
 Channel = 1;            
 Ready = false;
 DataAvailable = false;
 TCCR1A = 0x00;         // COM1A1=0, COM1A0=0 => Disconnect Pin OC1 from Timer/Counter 1 -- PWM11=0,PWM10=0 => PWM Operation disabled
 TCCR1B = 0x02;         // 16MHz clock with prescaler means TCNT1 increments every .5 uS (cs11 bit set)
 TIMSK1 = _BV(ICIE1);   // enable input capture interrupt for timer 1
 
}

void SelectChannel( int channel){
   digitalWrite(g,HIGH);  // turn the output off while twiddling select bits
   if(Channel == 1)
   {
      digitalWrite(a,LOW);
      digitalWrite(b,LOW);            
   }
   else if(Channel == 2)
   {
      digitalWrite(a,HIGH);
      digitalWrite(b,LOW);
   }
    else if(Channel == 3)
    {
      digitalWrite(a,LOW);
      digitalWrite(b,HIGH);
    }
    digitalWrite(c,LOW);   // c is always low

    digitalWrite(g,LOW);  // turn output on    
}


int GetChannelPulseWidth(uint8_t Channel) {
 // this is the access function for channel data
 int result;  
 if( DataAvailable  && (Channel > 0) && (Channel <=  MAX_CHANNELS)  ) {
    cli();             //disable interrupts
    result =  Pulses[Channel] ;
    sei();             // enable interrupts
 }
 else
    result = 0;        // return 0 if no valid pulse is available  

 return result;

}

void loop()                     // run over and over again
{
 
int pulsewidth;

 
     // print the decoder state
  if(DataAvailable == false)
     {// Serial.println("The decoder has not detected all channels ");  
}
  else  
       {

      PulseServo(1,GetChannelPulseWidth(1));
      delay(6);

      PulseServo(2,GetChannelPulseWidth(2));
      delay(6);
      PulseServo(3,GetChannelPulseWidth(3));
      delay(6);
  }    

 
 
 // now print the channel pulse widths
 // they should be 0 if the state is not ready
 for ( int i =1; i <=3; i++ ){ // print the status of the first three channels
     /*Serial.print("Channel ");
     Serial.print(i);
     Serial.print(" has width ");*/
     pulsewidth = GetChannelPulseWidth(i);
       //Serial.println(pulsewidth);
 }
 
   
 
}

void PulseServo(int channel,int pulsewidth)
{
 //   your code here to pulse the servo for the given channel with the given pulse width
 // you should check that pulsewidth is within the valid range for your servos

 int pin_num = 0;
 
 pin_num = channel + 1;

 if ((pulsewidth <= 2200) && (pulsewidth >= 600))
 {
   digitalWrite(pin_num, HIGH);  
   delayMicroseconds(pulsewidth);
   digitalWrite(pin_num, LOW);
 }

/*  Serial.print("chan ");
 Serial.print(channel);
 Serial.print("=");
 Serial.print(pulsewidth);
 Serial.print(", ");
 Serial.println();*/

}


by the way, can i use the timer1 for the RC and also the servo timing also? would it be possible?

mem

#47
Mar 12, 2008, 10:01 am Last Edit: Mar 12, 2008, 10:22 am by mem Reason: 1
Are the pulse widths stable? ( can you attach a few seconds worth of the debug output?)

The Timer1 counter is reset on the input pulses so can't be used for output as well. But the pulseServo function should work ok as is. Why not verify this by modifying GetChannelPulseWidth to just return a value from a potentiometer connected to an analog pin. The pulse width would be the analogRead value + 1000.  


Also, are you powering the servos from the arduino 5v line? If so, try it using an external 5v source for the servos.

axileon

#48
Mar 12, 2008, 10:50 am Last Edit: Mar 12, 2008, 12:06 pm by axileon Reason: 1
hi i posted the snapshot of the debug screen here....

http://www.axileon.com/blog/untitled.JPG

the pulsewidth is pretty consistent...

i am using an dc adaptor from the mains....

mem

#49
Mar 12, 2008, 11:11 am Last Edit: Mar 12, 2008, 11:15 am by mem Reason: 1
Try it with the interrupt code completly removed to be sure interrupts are not running.
make sure this line is removed: TIMSK1 = _BV(ICIE1);   // enable input capture interrupt

Just modify GetChannelPulseWidth to return hard coded values similar to those you just posted and see if you get the same servo behavour.

If so, it may be a power issue. If the problem goes away then we can look to see if the interrupt is causing the problem or if it is something else

You could also try a test with no serial print statments to verify that those are not affecting the timings.

axileon

hey i think i have found the solution to the problem.... i tried fiddling with the code..

power supply is alright... interrupt is not causing the problem.

the problem is with the delay function of arduino.. if i want to get very decent regular pulses, i have to use the delaymicroseconds function.

However, this creates another problem. delaymicroseconds disables interrupts.. i cant use it too.. so any more recommendations? i'm damn heck frustrated now.

mem

You could use timer2 to time the pulses at suitable precision (i.e. .5us per count, the same as timer1 in yr sketch). It will overflow every 128us  so you will need a variable that counts the number of overflows.  The logic could be something like this:

Set an output channel index counter to first channel

Get the pulse width for this channel      
   Divide pulse width (in microseconds) by 128 to set an overflow counter variable
   Take the integer remainder  (i.e. modulus) to get the output compare value for the timer when the overflow count has been reached the calculated value.
  Pulse the pin for this channel high,  reset the timer and enable interrupts
  In the timer2 interrupt handler, decrement the overflow counter when the counter reachs 256
  When the overflow counter reaches zero, set the output compare register to the remainder value you calculated earlier.
When the output compare interrupt occurs, set the channel pin low.
Repeat this loop for all channels

When all channels have been pulsed, you need to wait for the start of the next frame before repeating. (pulsing a typical analog servo with insufficient gap is not good for the servos)

One way of doing this is as follows:
   Store the millis() count just before you start pulsing the first channel
When all channels have been pulsed and any processing for the next frame is completed, note the millis() count. You want to call delay() for as many milliseconds as necessary for 20 milliseconds to have elapsed since your start of pulsing time. Note this interframe time delay is not critical and I doubt moderate variation would have any impact on the performance of your servos.

I hope these suggestions are  helpful.

axileon

hm.. but the specs for atmega168 says that timer2 is a 8 bit timer too....

u're meaning i can scale the clock for timer2 to be the same as timer1?

mem

#53
Mar 13, 2008, 03:19 pm Last Edit: Mar 13, 2008, 03:24 pm by mem Reason: 1
Yes, you can have a prescale of 8 giving 0.5us per count with 16mhz arduino on timer2 just like timer1. But, timer2 overflows in 128us, timer1 overflows in 32ms with the same prescale of 8.

axileon

Quote

Set an output channel index counter to first channel

Get the pulse width for this channel      
   Divide pulse width (in microseconds) by 128 to set an overflow counter variable
   Take the integer remainder  (i.e. modulus) to get the output compare value for the timer when the overflow count has been reached the calculated value.
  Pulse the pin for this channel high,  reset the timer and enable interrupts
  In the timer2 interrupt handler, decrement the overflow counter when the counter reachs 256
  When the overflow counter reaches zero, set the output compare register to the remainder value you calculated earlier.
When the output compare interrupt occurs, set the channel pin low.
Repeat this loop for all channels

When all channels have been pulsed, you need to wait for the start of the next frame before repeating. (pulsing a typical analog servo with insufficient gap is not good for the servos)

I hope these suggestions are  helpful.


hi, i tried implement your solution but i have difficulty getting the interrupts to work.

here is the code.

Code: [Select]
//#include <avr/io.h>
//#include <avr/interrupt.h>

#define INIT_TIMER_COUNT 0
#define RESET_TIMER2 TCNT2 = INIT_TIMER_COUNT

int servo = 3;
int servoRoll = 5;
int servoPitch = 6;
int servoYaw = 11;
int counter = 0;
int compare = 0;
 
ISR (TIMER2_OVF_vect)
{
 counter--;
 if (counter == 0)
 {
   OCR2A = compare;
   }
 
}

ISR (TIMER2_COMPA_vect)
{
digitalWrite(2,LOW);
}

void setup()
{
 Serial.begin(9600);

 pinMode(2, OUTPUT);
 pinMode(servo, OUTPUT);
 pinMode(servoRoll, OUTPUT);
 pinMode(servoPitch, OUTPUT);
 pinMode(servoYaw, OUTPUT);


 /* setup for timer 2 */

 TCCR2A = _BV(COM2A1) | _BV(COM2B1)| _BV(WGM21) | _BV(WGM20);  // clear both A on compare match, set fast PWM mode.
 TCCR2B = _BV(CS21); // set prescaler of 1024 for 8 bit timer - 0 to 255.  

 TIMSK2 = _BV(OCIE2A) | _BV(TOIE2) ; // enables the interrupt for the
/* end of setup for timer 2 */

}

void loop()
{
  int pulsewidth = 1500;

  counter = pulsewidth/128;
//   Serial.println(counter);
  compare = pulsewidth%128;
//  Serial.println(compare);
   
   
  digitalWrite(2,HIGH);

  RESET_TIMER2;  
 
  sei();
 
}


actually what you are suggesting to me is that I use the timer 2 to pulse the servos channel by channel right? so I am not using OCR2A's interrupt pin right?

mem

#55
Mar 17, 2008, 03:02 pm Last Edit: Mar 17, 2008, 03:15 pm by mem Reason: 1
Here is some code that is closer to what you want. It is untested and probably has errors that will need fixing. For example I am not sure what will happen if the output compare value is set for 0 (i.e. if it doesn't trigger immediately then your code needs to handle this)

You can test it with NBR_CHANNELS 1 and it will (or should when you get it going ;) ) just pulse the servo on pin 3
Code: [Select]


#define NBR_CHANNELS 4
#define DEFAULT_PULSE_WIDTH  1500

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 compare[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

ISR (TIMER2_OVF_vect)
{
 --counter[Channel];
 if (counter[Channel] == 0)
 {
   OCR2A = compare[Channel];  
   TIMSK2 = _BV(OCIE2A);  // enable the output compare interrupt.
 }  
}

ISR (TIMER2_COMPA_vect)
{
digitalWrite(pins[Channel],LOW);
Channel++;  // now increment to the next channel
if(Channel >= NBR_CHANNELS)  {    
   TIMSK2 = 0; // we have done all channels, so disable interrupts
   Channel = 0;      
}
else{
   digitalWrite(pins[Channel],HIGH); // pulse the next channel high
   TCNT2 = 0;  // reset the count;
   TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt
 }
}

void SetPulse(byte channel, int pulsewidth){
// store the values for the given channel  
     counter[channel] = pulsewidth / 128;    
     compare[channel] = pulsewidth% 128;  
}

void PulseServos(){
 // start the frame pulsing each servo in turn once
   Channel = 0;  // start from the first channel
   TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt
}


void setup()
{
 Serial.begin(9600);
 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
 }
 pinMode(2, OUTPUT);

 /* setup for timer 2 */
 TCCR2A = _BV(COM2A1) | _BV(COM2B1)| _BV(WGM21) | _BV(WGM20);  // clear both A on compare match, set fast PWM mode.
 TCCR2B = _BV(CS21); // set prescaler of 1024 for 8 bit timer - 0 to 255.  
}

void loop()
{
 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
  // you may want to also check that Channel equals 0 just to make sure            
}



You could add something like this in loop if you want to test setting the servo position via a pot.
     SetPulse( 0, analogRead(potPin) + 1000 );  // set pulse width on the first channel according to the pot value

axileon

hi.. i tried compiling the code.. but i keep gettting an error... i think there's something wrong with the variable name..

this error is expected primary expression before [ token;
Code: [Select]
ISR(TIMER2_OVF_vect)
{ <<<<------- error points to here
 --counter[Channels];
 if (counter[NBR_CHANNELS] == 0)
 {
   OCR2A = compare[Channel];  
   TIMSK2 = _BV(OCIE2A) // enable the output compare interrupt.
 }  
}


i've been cracking my brains but i dont see anything wwrong with the code?

mem

I just tried pasting the code I posted above in a sketch and it compiles ok. Try another cut and paste. If that doesn't work, paste the code you are compiling into a reply here and I will have a look.

axileon

hi... the i have response for channel 5,6 .. but the pulses are incorrect.... also channel 10 , the last channel i have a  HIGH signal thru out...

// todo the above delays need enhancing to detect millis rollover
 // note that it assumes that servos will have finished pulsing within 20ms
 // you may want to also check that Channel equals 0 just to make sure      

sorry i wish to know what do u mean by the paragraph above? cos i dont really understand it.

the code i have used is the same as the one that you have ...


mem

#59
Mar 18, 2008, 10:41 am Last Edit: Mar 18, 2008, 10:51 am by mem Reason: 1
Can you say a little more about how the pulses are incorrect. Also, did you mean pin 11 rather than 10? what about pin 3?  

Try it with just one channel (#define NBR_CHANNELS 1) to see what that does.

You could also put some debugging statements in the code to see whats happening. Here is an example, but change this as required so you can see what the code is doing:
Edit: serial print statements updated with byte values cast to ints

Code: [Select]


#define NBR_CHANNELS 4
#define DEFAULT_PULSE_WIDTH  1500

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 compare[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

ISR (TIMER2_OVF_vect)
{
 --counter[Channel];
 if (counter[Channel] == 0)
 {
   OCR2A = compare[Channel];  
   TIMSK2 = _BV(OCIE2A);  // enable the output compare interrupt.
 }  
 Serial.print(" ISR count=");
 Serial.print((int)counter[Channel]);  
}

ISR (TIMER2_COMPA_vect)
{
digitalWrite(pins[Channel],LOW);
Channel++;  // now increment to the next channel
if(Channel >= NBR_CHANNELS)  {    
   TIMSK2 = 0; // we have done all channels, so disable interrupts
   Channel = 0;      
}
else{
   digitalWrite(pins[Channel],HIGH); // pulse the next channel high
   Serial.print(" ISR cmp=");
   Serial.println((int)TCNT2);
   TCNT2 = 0;  // reset the count;
   TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt
 
 }
}

void SetPulse(byte channel, int pulsewidth){
// store the values for the given channel  
     counter[channel] = pulsewidth / 128;    
     compare[channel] = pulsewidth% 128;  
}

void PulseServos(){
 // start the frame pulsing each servo in turn once
   Channel = 0;  // start from the first channel
   TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt
}


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 * 129)); // this should ensure that each of the channels have different values;
 }
 pinMode(2, OUTPUT);

 /* setup for timer 2 */
 TCCR2A = _BV(COM2A1) | _BV(COM2B1)| _BV(WGM21) | _BV(WGM20);  // clear both A on compare match, set fast PWM mode.
 TCCR2B = _BV(CS21); // set prescaler of 1024 for 8 bit timer - 0 to 255.

 for(int i=0; i < NBR_CHANNELS; i++) {
   Serial.print("Chan ");
   Serial.print(i);
   Serial.print(", Counter = ");
   Serial.print((int)counter[i]);
   Serial.print(", Compare = ");
   Serial.println((int)compare[i]);    
 }  
}

void loop()
{
 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
  // you may want to also check that Channel equals 0 just to make sure            
}




Note the increased the baud rate in that code. You may want it even higher to keep the processing time down in the interrupt handler.

Don't worry about things like millis rollover for now, those comments were to remind us to handle exceptional situations. We can easily address that when the code is working as expected.

Go Up