3 axis auto stabilized platform

Good to hear its starting to work :slight_smile:

The Channel variable is a channel number, but because there is nothing in this verison to indicate which channel is channel 1 then it will indicate channel 1 as the first channel it happens to detect. But the three channels should have different values if you are sending different values on your transmitter.

edit: I just reread what I wrote above and its not correct. The multiplexer will select the correct channel if its gated correctly.

Could you post the sketch that includes the multiplexer code.

alright. here it is.

  if(++Channel > MAX_CHANNELS)
            {
                    Channel = 1;       //reset the channel counter to 1      
                
                    if(Channel == 1)
                  {
                    digitalWrite(a,LOW);
                    digitalWrite(b,LOW);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                  } 
                     else if(Channel == 2)
                {
                    digitalWrite(a,HIGH);
                    digitalWrite(b,LOW);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                }  
                   else if(Channel == 3)
                  {
                   
                    digitalWrite(a,LOW);
                    digitalWrite(b,HIGH);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                  }
                    DataAvailable = true;  // this line is needed to indicate that the channel data is valid!!!! 
                }

the debug screen actually prints all 3 channels with the same value(channel 1)....

so, how do i actually configure the code to print all the 3 different channel values then?

hi i changed my code. i managed to get it working right now... just that there appear to be another problem.

i have connected the receiver in sequence 0,1,2

however the debug screen prints
chn 1 as receiver chn 2
chn 2 as receiver chn 0
chn 3 as receiver chn 1

Pulses[Channel] = ICR1 / TICKS_PER_uS;  // store pulse length as microsoeconds
               if(Channel == 1)
                  {
                    digitalWrite(a,LOW);
                    digitalWrite(b,LOW);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                  } 
                     else if(Channel == 2)
                {
                    digitalWrite(a,HIGH);
                    digitalWrite(b,LOW);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                }  
                   else if(Channel == 3)
                  {
                   
                    digitalWrite(a,LOW);
                    digitalWrite(b,HIGH);
                    digitalWrite(c,LOW);
                    digitalWrite(g,LOW);
                  }
                  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!!!! 
                }

also.. i'm using the serial servo program to update the servos.. but the gap in in between pulses is too long... do you know of a better way to pulse the servos accurately?

I composed the following while you where posting the above. Have a read of this while I read your post:

modify the code as below so that the multiplexer channel selection is outside the brackets. I have moved it to a separate function of convenience. This code also turns the output of the multiplexer off when selecting.

 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)



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    
}

edit: looks like you made the fix to move the Channel Selection outside the brackts. But you should add the code to gate the multiplexer off when writing the selection pins.

If you post your servo code will see if I can help. But in principle you want to update the servo every 20 milliseconds using whatever data is in the channel array after the DataAvailable flag is set to true

Note that the channel array is updated only every 40 ms or so but the servos should be pulsed every 20ms.

Here is a suggestion that I hope helps:

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

}

void loop()                     // run over and over again
{

   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);
   }  
}

hi... thanks for your help so far.. i managed to get it working already. :slight_smile:

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 pulse = 0;
  int pin_num = 0;
  
  pin_num = channel + 1; 
  pulse = pulsewidth;

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

}

however... the servo is a litter jerky..... i check the pulse on the scope.... when everything is stationary... the PWM seems a bit unstable.. the pulses keeps moving left right right left... though the time between pulses is still ok..

do you have any idea what is causing the pulses to be moving?

You could send the pulse width to the serial port so you can inspect what is being sent to the servo.

Something like this would do the trick

void PulseServo(int channel,int pulsewidth)
{
  // note that pulse variable is not needed and has been removed

  int pin_num = 0;
  pin_num = channel + 1; 

  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 
  }
      
  Serial.print("chan ");
  Serial.print(Channel);
  Serial.print("=");
  Serial.print(PulseWidth);
  Serial.print(", ");

}

hey. thanks.. actually i'm using timer 0 and timer 2 for do a PWM for the servos.... timer 1 will be used for the ICP.....

the code is as follow...

#include <avr/io.h> 
#include <avr/interrupt.h> 

int servoRoll = 6;
int servoPitch = 5;
int servoYaw = 11;

void setup()
{

  Serial.begin(9600);
 
  pinMode(servoRoll, OUTPUT);
  pinMode(servoPitch, OUTPUT);
  pinMode(servoYaw, OUTPUT);

/* setup for timer 0 */    
  TCCR0A = _BV(COM0A1) |  _BV(COM0B1) | _BV(WGM01) | _BV(WGM00);  // clear both A n B on compare match, set fast PWM mode.
  TCCR0B = _BV(CS02) | _BV(CS00); // set prescaler of 1024 for 8 bit timer - 0 to 255.  
  
  TIMSK0 = _BV(OCIE0B) | _BV(OCIE0A) | _BV(TOIE0) ; // enables the interrupt for the 
  
  OCR0A = OCR0B = 0x18; // 0x18 = 24 = 1.5 ms

/* end of setup for timer 0 */

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

  OCR2A = 0x18; // 0x18 = 24 = 1.5 ms

/* end of setup for timer 2 */
  
  sei();
  
}

void loop()
{
  cli();
  OCR2A= 16;
  sei();
  delay(17);
    cli();
  OCR2A = 32;
  sei();
  delay(17);

}

i discovered that i can only use 1 of the timers for this purpose.. not two. if i use two and one of them is correct, the other has something wrong with the pulse.

actually we cant keep changing the OCRXX values right? but my counter will overflow after 16 ms right? that means I should be able to vary the pulse for the motor right?

The resolution is pretty low using the 8 bit counters and that may explain why the servos are not smooth.

Any reason you can't use a simple delay instead of the timers for driving the servos similar to reply #33 above? You have at least 14 milliseconds to do any calculations between pulsing the servos and because your data is changing every two frames you can spread this over 28 milliseconds if you need to.

the waveform of the servo is too inconsistent if i use other methods rather than interrupt to generate them...

but i thought timers are supposed to be more accurate than if we use digitalwrite hi's and low's?

If an 8 bit timer count of 24 is 1.5ms then each step is over 60 microseconds. It should be within a microsecond if you use delayMicrosecond. Have you tried it using the delay instead of the timer?

i am aware of the low resolution if i use an 8 bit counter.. but my professor advises me to use interrupt for the timing of the servos....

i will try the delay thing again tomo... i tried it lasttime... the gap between pulses is too long... and its also quite jerky... maybe its the cycling of the timer..

if i throw in the code for the gyros.. then most probably everything will be haywire again...

If this is for a school project then keeping your professor happy is an important requirement :wink:

My suggestion is that this is a good time to try and determine how much processing time you need per frame. If this time is more than around 15 milliseconds but less than around 30ms then consider if it is acceptable to use two frames for your calculations (the channel inputs are only updated every other frame because of the way we have implemented the multiplexer). If you need a lot more than 15 milliseconds and the data does need to be refreshed every frame (20 ms), or you need more than 30ms to do your calculations using two frames, then you are getting close to exceeding the Arduino's processing capability even if you could run the servos and input capture all on interrupts.

for example, the following pulses the servos every 20ms and give around 30ms of calculation time every 50ms (two frames):

start of frame n
pulse three servos in sequence (4-6 ms)
start calculating new pulse width (15ms)
end of frame n

start of frame n+1
pulse three servos in sequence using previous frame data (4-6 ms)
finish calculating new pulse width (15ms)
end of frame n+1

repeat...

Hi, Sorry to join this thread so late in the piece. I am slowly making progress on a similar project but to stabilise a quad copter. I have gathered snippets of code and ideas from all sorts of places, most notably a UAVP project in Europe. It is worth a google of UAVP and a look at their site if you have not already done so. They make a lot of their code available and there are lots of clues there on how to decode the stream and in fact to deal with sensors you mentioned above. I am now at the stage where I have the raw RC 'stick' values captured and have mastered comms with the sensors. When you get to the point of dealing with PID code etc, would love to compare notes.

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?

Hi, Sorry to join this thread so late in the piece. I am slowly making progress on a similar project but to stabilise a quad copter. I have gathered snippets of code and ideas from all sorts of places, most notably a UAVP project in Europe. It is worth a google of UAVP and a look at their site if you have not already done so. They make a lot of their code available and there are lots of clues there on how to decode the stream and in fact to deal with sensors you mentioned above. I am now at the stage where I have the raw RC 'stick' values captured and have mastered comms with the sensors. When you get to the point of dealing with PID code etc, would love to compare notes.

hi.. thanks for replying.. i took a look at hte opensource thing... actually i'm quite rushing for time to finish the project.. i look at the source code of the quadcopter.. but its too complicated for me. haha..

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?

  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.

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

arlight.. here's the code..

#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?

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.

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....