Main Page | Modules | Data Structures | Directories | File List | Data Fields | Globals | Related Pages

# servo.c

Go to the documentation of this file.

    00001 /*! \file servo.c \brief Interrupt-driven RC Servo function library. */
    00002 //*****************************************************************************
    00003 //
    00004 // File Name    : 'servo.c'
    00005 // Title        : Interrupt-driven RC Servo function library
    00006 // Author       : Pascal Stang - Copyright (C) 2002
    00007 // Created      : 7/31/2002
    00008 // Revised      : 8/02/2002
    00009 // Version      : 1.0
    00010 // Target MCU   : Atmel AVR Series
    00011 // Editor Tabs  : 4
    00012 //
    00013 // This code is distributed under the GNU Public License
    00014 //      which can be found at http://www.gnu.org/licenses/gpl.txt
    00015 //
    00016 //*****************************************************************************
    00017
    00018 #ifndef WIN32
    00019     #include <avr/io.h>
    00020 #endif
    00021
    00022 #include "global.h"
    00023 #include "servo.h"
    00024
    00025 // Program ROM constants
    00026
    00027 // Global variables
    00028 // servo channel registers
    00029 u16 ServoPosTics;
    00030 u16 ServoPeriodTics;
    00031 u08 ServoChannel;
    00032 ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
    00033
    00034 // functions
    00035
    00036 //! initializes software PWM system
    00037 void servoInit(void)
    00038 {
    00039     u08 channel;
    00040     // disble the timer1 output compare A interrupt
    00041     cbi(TIMSK, OCIE1A);
    00042     // set the prescaler for timer1
    00043     timer1SetPrescaler(TIMER_CLK_DIV256);
    00044     // attach the software PWM service routine to timer1 output compare A
    00045     timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
    00046     // enable and clear channels
    00047     for(channel=0; channel<SERVO_NUM_CHANNELS; channel++)
    00048     {
    00049         // set minimum position as default
    00050         ServoChannels[channel].duty = SERVO_MIN;
    00051         // set default port and pins assignments
    00052         ServoChannels[channel].port = _SFR_IO_ADDR(SERVO_DEFAULT_PORT);
    00053         //ServoChannels[channel].port = (unsigned char)&SERVO_DEFAULT_PORT;
    00054         ServoChannels[channel].pin = (1<<channel);
    00055         // set channel pin to output
    00056         // THIS IS OBSOLETED BY THE DYNAMIC CHANNEL TO PORT,PIN ASSIGNMENTS
    00057         //outb(SERVODDR, inb(SERVODDR) | (1<<channel));
    00058     }
    00059     // set PosTics
    00060     ServoPosTics = 0;
    00061     // set PeriodTics
    00062     ServoPeriodTics = SERVO_MAX*9;
    00063     // set initial interrupt time
    00064     u16 OCValue;
    00065     // read in current value of output compare register OCR1A
    00066     OCValue =  inb(OCR1AL);     // read low byte of OCR1A
    00067     OCValue += inb(OCR1AH)<<8;  // read high byte of OCR1A
    00068     // increment OCR1A value by nextTics
    00069     OCValue += ServoPeriodTics;
    00070     // set future output compare time to this new value
    00071     outb(OCR1AH, (OCValue>>8));         // write high byte
    00072     outb(OCR1AL, (OCValue & 0x00FF));   // write low byte
    00073     // enable the timer1 output compare A interrupt
    00074     sbi(TIMSK, OCIE1A);
    00075 }
    00076
    00077 //! turns off software PWM system
    00078 void servoOff(void)
    00079 {
    00080     // disable the timer1 output compare A interrupt
    00081     cbi(TIMSK, OCIE1A);
    00082     // detach the service routine
    00083     timerDetach(TIMER1OUTCOMPAREA_INT);
    00084 }
    00085
    00086 //! set port and I/O pin for channel
    00087 void servoSetChannelIO(u08 channel, u08 port, u08 pin)
    00088 {
    00089     ServoChannels[channel].port = port;
    00090     ServoChannels[channel].pin = (1<<(pin&0x07));
    00091 }
    00092
    00093 //! set servo position on channel
    00094 void servoSetPosition(u08 channel, u08 position)
    00095 {
    00096     // input should be between 0 and SERVO_POSITION_MAX
    00097     u16 pos_scaled;
    00098     // calculate scaled position
    00099     pos_scaled = ((u16)position*(SERVO_MAX-SERVO_MIN)/SERVO_POSITION_MAX)+SERVO_MIN;
    00100     // set position
    00101     servoSetPositionRaw(channel, pos_scaled);
    00102 }
    00103
    00104 //! get servo position on channel
    00105 u08 servoGetPosition(u08 channel)
    00106 {
    00107     return (u08)( ((servoGetPositionRaw(channel)-SERVO_MIN)*SERVO_POSITION_MAX)/(SERVO_MAX-SERVO_MIN) );
    00108 }
    00109
    00110 //! set servo position on channel (raw unscaled format)
    00111 void servoSetPositionRaw(u08 channel, u16 position)
    00112 {
    00113     // bind to limits
    00114     position = MAX(position, SERVO_MIN);
    00115     position = MIN(position, SERVO_MAX);
    00116     // set position
    00117     ServoChannels[channel].duty = position;
    00118 }
    00119
    00120 //! get servo position on channel (raw unscaled format)
    00121 u16 servoGetPositionRaw(u08 channel)
    00122 {
    00123     return ServoChannels[channel].duty;
    00124 }
    00125
    00126 void servoService(void)
    00127 {
    00128     u16 nextTics;
    00129
    00130     if(ServoChannel < SERVO_NUM_CHANNELS)
    00131     {
    00132         // turn off current channel
    00133         outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) & ~(ServoChannels[ServoChannel].pin));
    00134     }
    00135
    00136     // next channel
    00137     ServoChannel++;
    00138
    00139     if(ServoChannel != SERVO_NUM_CHANNELS)
    00140     {
    00141         // loop to channel 0 if needed
    00142         if(ServoChannel > SERVO_NUM_CHANNELS)   ServoChannel = 0;
    00143         // turn on new channel
    00144         outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) | (ServoChannels[ServoChannel].pin));
    00145         // schedule turn off time
    00146         nextTics = ServoChannels[ServoChannel].duty;
    00147     }
    00148     else //(Channel == SERVO_NUM_CHANNELS)
    00149     {
    00150         // ***we could save time by precalculating this
    00151         // schedule end-of-period
    00152         nextTics = ServoPeriodTics-ServoPosTics;
    00153     }
    00154
    00155     // schedule next interrupt
    00156     u16 OCValue;
    00157     // read in current value of output compare register OCR1A
    00158     OCValue =  inb(OCR1AL);     // read low byte of OCR1A
    00159     OCValue += inb(OCR1AH)<<8;  // read high byte of OCR1A
    00160     // increment OCR1A value by nextTics
    00161     OCValue += nextTics;
    00162 //  OCR1A+=nextTics;
    00163     // set future output compare time to this new value
    00164     outb(OCR1AH, (OCValue>>8));         // write high byte
    00165     outb(OCR1AL, (OCValue & 0x00FF));   // write low byte
    00166     // set our new tic position
    00167     ServoPosTics += nextTics;
    00168     if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
    00169 }