ServoController

I’ve had this one finished for a while now and you can see it in action in previous posts, controlling the quadrapod. I revamped the code recently. The Idea came after a hackaday item about a servo-controller:
http://www.societyofrobots.com/member_tutorials/node/25
This controller can only handle 8 servo’s, which is unacceptable… But it did give me an idea about how to cram all the servo-signal generation into interrupt routines.

I left all communication out of this snippet since this is about generating servo-signals. You’ll notice the entire main loop is empty, so there’s plenty of room to use your own preferred way of exchanging data.

This version handles 2 servo-channels per “interval”, by increasing this to 3 or even 4 you can address up to 32 servo’s. I only use 12 servo’s at the moment so the code snippet reflects that.

Source code after the break.

The comments in the code assume a decent level of understanding about servo’s

//name: servocontroller.c
//Author: Michiel van der Coelen
//contact: Michiel.van.der.coelen@gmail.com
//date: 2011-02-06
//tabsize: 2
//width: 80
//This code is distributed under the GNU Public License
//which can be found at http://www.gnu.org/licenses/gpl.txt
 
 
//MCU = atmega32
//Lfuse = EE
//Hfuse = DA (bootsector size 1024)
//F_CPU=12000000 //defined in makefile
//all comments assume this clock frequency, you'll have to recalculate most
//values when you change it
//when changing MCU, timer registers may have different names.
 
 
/*basic description
-------------------------------
Timer1 triggers an interrupt on an interval slightly larger than the maximum
pulsewidth of the servo-signals (so a bit more than 2 ms).
On this interrupt, TWO servo channels are set high and 
timer0 is started (much faster than timer1). Timer0 also triggers an interrupt.
Each interrupt, a counter (Tic) is increased which represents a time-frame.
When the counter equals a high channel's 'stop' variable (Stop1 or Stop2),
the channel is pulled low. 
On the next interrupt from timer1, the next TWO channels are set high,
and the cycle repeats.
After the last TWO channels are set high and timer1 has another interrupt,
the RemainingTime till the end of the period is loaded into the timer.
*/
#include <avr/io.h> 
#include <avr/interrupt.h>
 
#define SET(x,y) (x|=(1<<y))
#define CLR(x,y) (x&=(~(1<<y)))
#define CHK(x,y) (x&(1<<y)) 
#define TOG(x,y) (x^=(1<<y))
 
#define SERVO_PORT_FIRST8 PORTA
#define SERVO_PORT_SECOND8 PORTC
//you can use these two in your 'init_channels()' function
//or you can define 16 unique ports if you want.
 
#define SERVO_AMOUNT 12 
//even only, up to 16.
//You'll have to edit 'init_channels()' as well when changing this.
 
//used by timer0 (12Mhz/250)
#define SERVO_MAX_PULSE 96 // 200 us
#define SERVO_MIN_PULSE 48 // 100 us
 
//used by timer1 (12Mhz/256):
#define SERVO_PERIOD 937 //1999 us (50 Hz)
 
#define SERVO_BASE 96  //205 us
//this is the time till the next TWO servo channels are handled
 
 
typedef struct struct_multiservo{
  uint16_t stop;
  uint8_t port;
  uint8_t pin;
} ServoChannel_type;
 
//-----------------------------------------------------------------------------
//Global Variables
//-----------------------------------------------------------------------------
void (*jump_to_boot)(void) = 0x0C00;
uint8_t Index; //the current servochannel we're handling
ServoChannel_type ServoChannel[SERVO_AMOUNT];
uint8_t Stop1; //variables to hold the time at which to lower the channel
uint8_t Stop2;
uint8_t Tic; //timeframe counter
 
uint16_t RemainingTime; //time left after all servo's have been handled
 
 
//-----------------------------------------------------------------------------
//Functions
//-----------------------------------------------------------------------------
void init_servo_timers(void){
  TCCR1A =0;
  TCCR1B =(1<<CS12)|(1<<WGM12);
  OCR1A = SERVO_BASE;
  TCCR0 = 0;
  OCR0 = 250;
}
 
void init_channels(void){
//you need to set your own DDR
  register uint8_t i;
  for(i=0;i<8;i++){
    ServoChannel[i].port = _SFR_IO_ADDR(SERVO_PORT_FIRST8);
    ServoChannel[i].pin = i;
    ServoChannel[i].stop =((SERVO_MAX_PULSE + SERVO_MIN_PULSE) /2);
  }
  for(i=8;i<SERVO_AMOUNT;i++){
    ServoChannel[i].port = _SFR_IO_ADDR(SERVO_PORT_SECOND8);
    ServoChannel[i].pin = i - 6;
    ServoChannel[i].stop = ((SERVO_MAX_PULSE + SERVO_MIN_PULSE) /2);
  }
 
}
 
//-----------------------------------------------------------------------------
//MAIN
//-----------------------------------------------------------------------------
//usb initialization
int main() {
  //initialize servo channels and timers
  init_channels();
  init_servo_timers();
  SET(TIMSK, OCIE1A);
  RemainingTime = SERVO_PERIOD - (SERVO_BASE * (SERVO_AMOUNT)/2);
  //servo port data directions
  DDRA = 0xFF;
  DDRC |= (1<<PC2)|(1<<PC3)|(1<<PC4)|(1<<PC5);
  //start the signal generation
  sei();
//-----------------------------------------------------------------------------
//MAIN LOOP
//-----------------------------------------------------------------------------
while(1){
 
  //I don't know... do something usefull.
 
  }//main loop end
}//main end
 
//timer0 interrupt routine
ISR(TIMER0_COMP_vect){
  //increase timeframe counter
  Tic++;
  //check if we need to pull a channel low
  Index--;
  if(Tic == Stop2) {
    CLR(_SFR_IO8(ServoChannel[Index].port), ServoChannel[Index].pin);
  }
  Index--;
  if(Tic == Stop1) {
    CLR(_SFR_IO8(ServoChannel[Index].port), ServoChannel[Index].pin);
  }
  Index++;
  Index++;
}
 
//timer1 interrupt routine
ISR(TIMER1_COMPA_vect){
  //stop and reset timer0 and the counter
  CLR(TIMSK, OCIE0);
  TCCR0 = 0;
  TCNT0=0;
  Tic=0;
 
  //restore the base interval if this is the first set of channels
  if (Index == 0) OCR1A = SERVO_BASE;
 
  if(Index != SERVO_AMOUNT) {
    //set TWO servo channels high
    SET(_SFR_IO8(ServoChannel[Index].port), ServoChannel[Index].pin);
    Stop1 = ServoChannel[Index].stop;
    Index++;
 
    SET(_SFR_IO8(ServoChannel[Index].port), ServoChannel[Index].pin);
    Stop2 = ServoChannel[Index].stop;
    Index++;
    //start timer0 again
    TCCR0 = (1<<CS00)|(1<<WGM01);
    SET(TIMSK, OCIE0);
  } else { //we've done all servo channels, wait for next period
    Index=0;
    OCR1A = RemainingTime; 
  }
}
This entry was posted in avr C. Bookmark the permalink.

Leave a Reply

Your email address will not be published.

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong> <pre lang="" line="" escaped="">