Go Down

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

axileon

hi.. i tried changing the #define NBR_CHANNELS 4 to #define NBR_CHANNELS 1
it doesnt work unless i change this line

byte pins[NBR_CHANNELS] = {servo}

however pin 3has no output.... i wonder is it because timer 2 affect pin 3 and 11?

also.. i note the serialprint debug statements are printing symbols instead of numbers.. its because its defined as byte right?


mem

#61
Mar 18, 2008, 11:17 am Last Edit: Mar 18, 2008, 12:02 pm by mem Reason: 1
We don't want the timer to control the pins in hardware, so this needs to be turned off. But one problem that needs to be fixed first is that the interrupt code is changing the values of the counter and compare arrays so the timing values are only correct the first time through. We need to use a new variable to decrment in the interrupt handler.

Edit: added code to use counter variable:

Code: [Select]

volatile byte ISRCount;        // counter used in the interrupt routines;  << add this

ISR (TIMER2_OVF_vect)
{

 if (++ISRCount > counter[Channel] )   //<< change ISR code to increment this variable
 {
   OCR2A = compare[Channel];  
   TIMSK2 = _BV(OCIE2A);  // enable the output compare interrupt.
 }  
 
}

ISR (TIMER2_COMPA_vect)
{
Channel++;  // now increment to the next channel
ISRCount = 0; // reset the isr iteration counter          << add this line here
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  
 }
}


This will fix one of the issues with the current code, I expect there will be more to do to get it going.

axileon

right now i'm having a pulse of 1 us for every 128 us... for channel 3.

there seem to be another pulse that is appearing beside that 1us pulse... so its like 2 pulses every 128 us.

the rest of the channels all are high all the way...


mem

#63
Mar 18, 2008, 01:26 pm Last Edit: Mar 18, 2008, 01:31 pm by mem Reason: 1
This code should get you going. The major problem was the digitalWrites were not being done in the correct place. I also simplifed the interrupt code.

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
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 - compare[Channel];    // yes, so we count the remainder here
// you may want to rename 'compare' to 'remainder' if that seems clearer                            
 }  
 else if(ISRCount > counter[Channel])
 {
        // here if we have finished pulsing this channel !          
     digitalWrite(pins[Channel],LOW); // pulse this channel low
     Channel++;  // now increment to the next channel
     ISRCount = 0; // reset the isr iteration counter
     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      
 }  
}

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
   ISRCount = 0;  // reset the value of the ISR counter;
   digitalWrite(pins[Channel],HIGH); // pulse the first channel high
   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 */
 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
  // you may want to also check that Channel equals 0 just to make sure            
}


Still left to do is to check that the counting is done correctly i.e. that we do the correct number of iterations of the compare for the given pulse width.

After that, we can make add the code to handle the case where millis() rolls over back to zero.


axileon

sure. thanks a lot. i will try it tom morning..

anyway.. i wish to know why must we increase the baud rate? what effect does it has on the response of the program?

if i integrate it with my gyros at the latter stage. will it affect it? and also the RC signal?

mem

#65
Mar 18, 2008, 03:42 pm Last Edit: Mar 18, 2008, 03:53 pm by mem Reason: 1
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: [Select]


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


axileon

#66
Mar 19, 2008, 03:19 am Last Edit: Mar 19, 2008, 05:06 am by axileon Reason: 1
sorry. i have something to ask u about this section of the code.

Code: [Select]
if (++ISRCount == counter[Channel] ) // are we on the final iteration for this channel
 {
   //TCNT2 = 256 - remainder[Channel];   // yes, so count down the remainder
    TCNT2 = remainder[Channel];
 }


since we are using a remainder and counter, shouldnt the code be like what I have written in the above? we should be counting up instead of down right? since the counter is incrementing instead of decrementing... ?

i dont really understand this part of the code..can you explain it to me?

Code: [Select]
delay( endtime - millis()); // wait until end of frame  


oh by the way.. the pulses look fine to me. and i have managed to integrate the servo and the R/C together! ;D

thanks a lot.. now my next stage will be to integrate the gyros with the whole thing....

axileon

i have combined the whole program but the R/C controls are good... but the gyro is causing the whole pulsewidth to go crazy...

is there a way to smooth the gyro data more efficiently?? i'm just doing an averaging of the incoming data..

Code: [Select]
int digital_smooth(int rawIn, int *smooth_array)
{
 long total = 0;                            // the running total

 static int i;
 int j;
 
 i = (i + 1) % no_of_samples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
 smooth_array[i] = rawIn;
 
 for ( j = 0; j < no_of_samples; j++)
 {
   total += smooth_array[j];
 }
 
 return total/no_of_samples;
 
}

mem

#68
Mar 19, 2008, 03:14 pm Last Edit: Mar 19, 2008, 03:28 pm by mem Reason: 1
That code looks like it should work, but the values will be out of range until you fill up the array. You can fix that with a static variable called count that increments on each call until it equals  the array size. count holds the number of valid entries in the array so returning total/count  would give the correct results even when the array is not full.

Code: [Select]

int count = 0; //  this variable holds the number of samples added to the array
#define SAMPLE_ARRAY_SIZE ?  // this holds the size of the arrry

int digital_smooth(int rawIn, int *smooth_array)
{
 long total = 0;                            // the running total

 static int i;
 int j;
 
  i = (i + 1) % SAMPLE_ARRAY_SIZE;     // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
 smooth_array[i] = rawIn;

 if(count < SAMPLE_ARRAY_SIZE)
    count++
 
 for ( j = 0; j < no_of_samples; j++)
 {
   total += smooth_array[j];
 }  
 return total/count;  
}


But if your data does need more smoothing then consider a  Kalman filter. This is a very sophisticated technique and it may be overkill for your application. You can read more about Kalman filters including some ATmega168 source code here: http://forum.sparkfun.com/viewtopic.php?t=6186

To answer your earlier questions: the first version I posted used the output compare handler to do the remainder. The current one uses the overflow handler for all the counting. It times the remainder by setting the count so that it will overflow after the remainder number of ticks. Because it overflows above 255, the initial count is set to 255-remainder. Perhaps the comment should read something like: ' count up the remainder number of ticks'

We want to wait 20ms after starting to pulse the first servos before we start over again pulsing the next frame. Endtime is set when the servos start pulsing to be the starting time plus 20ms.  After pulsing, we calculating the difference between the current time and the end time and wait for that period. But, I am working on a version that does the end of frame timing in the interrupt as well, so there would be no need to use any of that delay code or worry about millis rollover. I will post it when its done.

mem

#69
Mar 19, 2008, 07:02 pm Last Edit: Mar 19, 2008, 07:21 pm by mem Reason: 1
Here is updated code for running the servos that runs completely in the interrupt handler. The pulse width timing on this version is accurate to around one percent. I changed the names of some of the functions in preparation for turning this into a library.

Code: [Select]


#define NBR_CHANNELS 4                   // the maximum number of channels.  
#define MIN_PULSE_WIDTH      750         // the shortest pulse sent to a servo  
#define MAX_PULSE_WIDTH      2250        // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH  1500        // default pulse width at startup

#define FRAME_SYNC_INDEX   NBR_CHANNELS  // frame sync delay is last entry in array
#define FRAME_SYNC_PERIOD  20000         // total frame sync time in us
#define FRAME_SYNC_DELAY   (FRAME_SYNC_PERIOD - ( NBR_CHANNELS * DEFAULT_PULSE_WIDTH))


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+1];   // holds pulse width / 128
byte remainder[NBR_CHANNELS+1];   // holds pulse width % 128
byte pins[NBR_CHANNELS+1] = {servo,servoRoll,servoPitch,servoYaw};  //holds pins associated with servos

volatile uint8_t Channel = 0;  // counter holding the channel being pulsed
volatile uint8_t ISRCount;  // counter used in the interrupt routines;

ISR (TIMER2_OVF_vect)
{
 ++ISRCount; // increment the overlflow counter
 if (ISRCount == counter[Channel] ) // are we on the final iteration for this channel
 {
     TCNT2 = remainder[Channel];   // yes, set count for overflow after remainder ticks
 }  
 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;    // reset the clock counter register
     if(Channel < NBR_CHANNELS){           // check if we need to pulse this channel    
         digitalWrite(pins[Channel],HIGH); // not done yet so pulse the next channel high  
     }
     else if(Channel == NBR_CHANNELS){     // check if this is the synch delay  
           // all channels pulsed, this is the sync delay, do nothing in this version
     }
     else {
        Channel = 0; // all done so start over              
         digitalWrite(pins[Channel],HIGH); // pulse the first channel high
     }
  }  
}

void ServoWrite(byte channel, unsigned int pulsewidth){
// calculate and store the values for the given channel
  if(channel <= NBR_CHANNELS) {   // ensure channel is valid
     if (channel < FRAME_SYNC_INDEX){    // if not frame sync, ensure pulse width is valid
        if( pulsewidth < MIN_PULSE_WIDTH )
            pulsewidth = MIN_PULSE_WIDTH;
        else if( pulsewidth > MAX_PULSE_WIDTH )
          pulsewidth = MAX_PULSE_WIDTH;      
     }
     counter[channel] = pulsewidth / 128;    
     remainder[channel] = 255 - (2 * (pulsewidth - (counter[channel] * 128)));  // the number of 0.5us ticks for timer overflow        
  }  
}

unsigned int ServoRead(byte channel){  
// returns the pulse width for the given channel  
  unsigned int pulsewidth = counter[channel] * 128 ;    
  pulsewidth +=  ((-remainder[channel] - 255) / 2);
  return pulsewidth;  
}

void ServoActivate(){
   for(int i=0; i < NBR_CHANNELS; i++) {
      pinMode( pins[i], OUTPUT) ;  // set servo pins to output
        ServoWrite(i, DEFAULT_PULSE_WIDTH);  // store default values          
   }
   ServoWrite(FRAME_SYNC_INDEX, FRAME_SYNC_DELAY);  // store the frame sync period  
 
   Channel = 0;  // start from the first channel  
   ISRCount = 0;  // clear the value of the ISR counter;
   
   /* setup for timer 2 */
   TIMSK2 = 0;  // disable interrupts
   TCCR2A = 0;  // normal counting mode
   TCCR2B = _BV(CS21); // set prescaler of 8
   TCNT2 = 0;     // clear the timer2 count
   TIFR2 = _BV(TOV2);  // clear pending interrupts;
   TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt            
}

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

 for(int i=0; i < NBR_CHANNELS; i++) {
     ServoWrite(i, 1000 + (i*250));    // test pulses from 1ms to 1.75ms in 250us steps              
 }
 ServoActivate();  // start the interrupt handler pulsing the servos
}

void loop()
{
 unsigned int val = analogRead(0);  // use a pot to set the pulse width for the first servo
 ServoWrite(0,val + 1000);  // write the pot value + 1000 to the first servo
}

axileon

sorry.. what is the servoRead function used for in the sketch above? is it for the RC data?

mem

#71
Mar 20, 2008, 01:33 pm Last Edit: Mar 20, 2008, 01:36 pm by mem Reason: 1
ServoRead returns the pulse width that has been set for that channel. it will be the last value written, or the default value if ServoWrite has not been called for this channel.

I have included it so that the library I am making will have a similar interface to the existing arduino servo libraries. Its useful if an application wants to either check the current value or to increase or decrease it by a fixed amount, for example the following code in loop would slowly pan a servo from its current position to the minimum  position :
    int newValue = ServoRead(chan) - 10;
    if( newValue >= MINIMUM_PULSE_WIDTH)
       ServoWrite(chan, newValue);
    delay(20);


axileon

so you mean, its something like getting the existing pulsewidth that the servo is getting currently?

by the way, i tested your latest code. works like a charm

right now i'm working on the testing of the R/C signals together with your latest servo code. however, the different variable numbers is giving me an headache. i think it has to do with the channel variable..


mem

#73
Mar 20, 2008, 02:03 pm Last Edit: Mar 20, 2008, 02:07 pm by mem Reason: 1
Exactly, Read returns the existing pulsewidth that is being pulsed to the servo

I have modified the code since I last posted to get it closer the version I will be using for the library. It now has channels starting from 1 (it uses chan 0 for the sync delay) . The library code will support  channel numbers or simple servo variables.


Something like this:
#include <ServoTimer2>

ServoTimer2 servoRoll;
ServoTimer2 servoPitch;
ServoTimer2 servoYaw;


In setup:
 servoRoll.Activate(rollPin);
 servoPitch.Activate(pitchPin);
 servoYaw.Activate(yawPin);


in loop:

  servoRoll.Write(analogRead(0) + 1000 ); // set the values from pots
  servoPitch.Write(analogRead(1)+ 1000 ) ;
  servoYaw.Write(analogRead(2)  + 1000 );


 
Additional methods using channel number arguments would be for apps where that is more convenient

mem

#74
Mar 21, 2008, 06:19 pm Last Edit: Mar 21, 2008, 06:19 pm by mem Reason: 1
Here is the first cut of the library if you want to use it. These file need to be in a new directory called ServoTimer2 in the same subdirectory as the other Arduino libraries.
Header:
Code: [Select]

/*
 ServoTimer2.h - Interrupt driven Servo library for Arduino using Timer2- Version 0.1
 Copyright (c) 2008 Michael Margolis.  All right reserved.

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

/*
 This library uses Timer2 to drive up to 8 servos using interrupts so no refresh activity is required from within the sketch.
 The usage and method naming is similar to the Arduino software servo library http://www.arduino.cc/playground/ComponentLib/Servo
 except that pulse width is in microseconds for greater accuracy rather than degrees.
 (A future version may also support degrees if this was required)
 
 A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
 The servo is pulsed in the background to the value most recently written using the write() method

 Note that analogWrite of PWM on pins 3 and 11 is disabled when the first servo is attached

 The methods are:

  ServoTimer2 - Class for manipulating servo motors connected to Arduino pins.

  attach()    - Attaches a servo motor to an i/o pin.

  write()     - Sets the servo pulse width in microseconds.

  read()      - Gets the last written servo pulse width in microseconds.

  attached()  - Returns true if there is a servo attached.

  detach()    - Stops an attached servos from pulsing its i/o pin.
 

The library takes about 824 bytes of program memory and 32+(1*servos) bytes of SRAM.
The pulse width timing is accurate to within 1%

*/

// ensure this library description is only included once
#ifndef ServoTimer2_h
#define ServoTimer2_h

#include <inttypes.h>
#include <wiring.h>

#define MIN_PULSE_WIDTH       750        // the shortest pulse sent to a servo  
#define MAX_PULSE_WIDTH      2250        // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH  1500        // default pulse width when servo is attached
#define FRAME_SYNC_PERIOD   20000        // total frame duration in microseconds
#define NBR_CHANNELS 8                   // the maximum number of channels, don't change this

typedef struct  {
     uint8_t nbr        :5 ;  // a pin number from 0 to 31
     uint8_t isActive   :1 ;  // false if this channel not enabled, pin only pulsed if true
  } ServoPin_t   ;  

typedef struct {
 ServoPin_t Pin;
 byte counter;
 byte remainder;
}  servo_t;

class ServoTimer2
{
 public:
     // constructor:
     ServoTimer2();

     uint8_t attach(int);     // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
                              // the attached servo is pulsed with the current pulse width value, (see the write method)
   void detach();
   void write(int);         // store the pulse width in microseconds (between MIN_PULSE_WIDTH and MAX_PULSE_WIDTH)for this channel
   int read();                    // returns current pulse width in microseconds for this servo
     boolean attached();      // return true if this servo is attached
private:
      uint8_t chanIndex;      // index into the channel data for this servo

};


// the following ServoArrayT2 class is not implimented in the first version of this library
class ServoArrayT2
{
 public:
     // constructor:
     ServoArrayT2();

     uint8_t attach(int);     // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
                              // channels are assigned consecutively starting from 1
                              // the attached servo is pulsed with the current pulse width value, (see the write method)
   void detach(int);        // detach the servo on the given channel
     void write(int,int);     // store the pulse width in microseconds (between MIN_PULSE_WIDTH and MAX_PULSE_WIDTH)for the given channel
   int read(int);                    // returns current pulse width in microseconds for the given channel
     boolean attached(int);   // return true if the servo on the given channel is attached
private:
      uint8_t chanIndex;      // index into the channel data for this servo

};

#endif


Source code in next post:

Go Up