?lang_form? ?lang_select? ?lang_submit? ?lang_endform?
{HEADER END}
{BLAME START}

library

?curdirlinks? -

Blame information for rev 6

Line No. Rev Author Line
1 6 kaklik /*! \file servo.c \brief Interrupt-driven RC Servo function library. */
2 //*****************************************************************************
3 //
4 // File Name : 'servo.c'
5 // Title : Interrupt-driven RC Servo function library
6 // Author : Pascal Stang - Copyright (C) 2002
7 // Created : 7/31/2002
8 // Revised : 8/02/2002
9 // Version : 1.0
10 // Target MCU : Atmel AVR Series
11 // Editor Tabs : 4
12 //
13 // This code is distributed under the GNU Public License
14 // which can be found at http://www.gnu.org/licenses/gpl.txt
15 //
16 //*****************************************************************************
17  
18 #ifndef WIN32
19 #include <avr/io.h>
20 #endif
21  
22 #include "global.h"
23 #include "servo.h"
24  
25 // Program ROM constants
26  
27 // Global variables
28 // servo channel registers
29 u16 ServoPosTics;
30 u16 ServoPeriodTics;
31 u08 ServoChannel;
32 ServoChannelType ServoChannels[SERVO_NUM_CHANNELS];
33  
34 // functions
35  
36 //! initializes software PWM system
37 void servoInit(void)
38 {
39 u08 channel;
40 // disble the timer1 output compare A interrupt
41 cbi(TIMSK, OCIE1A);
42 // set the prescaler for timer1
43 timer1SetPrescaler(TIMER_CLK_DIV256);
44 // attach the software PWM service routine to timer1 output compare A
45 timerAttach(TIMER1OUTCOMPAREA_INT, servoService);
46 // enable and clear channels
47 for(channel=0; channel<SERVO_NUM_CHANNELS; channel++)
48 {
49 // set minimum position as default
50 ServoChannels[channel].duty = SERVO_MIN;
51 // set default port and pins assignments
52 ServoChannels[channel].port = _SFR_IO_ADDR(SERVO_DEFAULT_PORT);
53 //ServoChannels[channel].port = (unsigned char)&SERVO_DEFAULT_PORT;
54 ServoChannels[channel].pin = (1<<channel);
55 // set channel pin to output
56 // THIS IS OBSOLETED BY THE DYNAMIC CHANNEL TO PORT,PIN ASSIGNMENTS
57 //outb(SERVODDR, inb(SERVODDR) | (1<<channel));
58 }
59 // set PosTics
60 ServoPosTics = 0;
61 // set PeriodTics
62 ServoPeriodTics = SERVO_MAX*9;
63 // set initial interrupt time
64 u16 OCValue;
65 // read in current value of output compare register OCR1A
66 OCValue = inb(OCR1AL); // read low byte of OCR1A
67 OCValue += inb(OCR1AH)<<8; // read high byte of OCR1A
68 // increment OCR1A value by nextTics
69 OCValue += ServoPeriodTics;
70 // set future output compare time to this new value
71 outb(OCR1AH, (OCValue>>8)); // write high byte
72 outb(OCR1AL, (OCValue & 0x00FF)); // write low byte
73 // enable the timer1 output compare A interrupt
74 sbi(TIMSK, OCIE1A);
75 }
76  
77 //! turns off software PWM system
78 void servoOff(void)
79 {
80 // disable the timer1 output compare A interrupt
81 cbi(TIMSK, OCIE1A);
82 // detach the service routine
83 timerDetach(TIMER1OUTCOMPAREA_INT);
84 }
85  
86 //! set port and I/O pin for channel
87 void servoSetChannelIO(u08 channel, u08 port, u08 pin)
88 {
89 ServoChannels[channel].port = port;
90 ServoChannels[channel].pin = (1<<(pin&0x07));
91 }
92  
93 //! set servo position on channel
94 void servoSetPosition(u08 channel, u08 position)
95 {
96 // input should be between 0 and SERVO_POSITION_MAX
97 u16 pos_scaled;
98 // calculate scaled position
99 pos_scaled = ((u16)position*(SERVO_MAX-SERVO_MIN)/SERVO_POSITION_MAX)+SERVO_MIN;
100 // set position
101 servoSetPositionRaw(channel, pos_scaled);
102 }
103  
104 //! get servo position on channel
105 u08 servoGetPosition(u08 channel)
106 {
107 return (u08)( ((servoGetPositionRaw(channel)-SERVO_MIN)*SERVO_POSITION_MAX)/(SERVO_MAX-SERVO_MIN) );
108 }
109  
110 //! set servo position on channel (raw unscaled format)
111 void servoSetPositionRaw(u08 channel, u16 position)
112 {
113 // bind to limits
114 position = MAX(position, SERVO_MIN);
115 position = MIN(position, SERVO_MAX);
116 // set position
117 ServoChannels[channel].duty = position;
118 }
119  
120 //! get servo position on channel (raw unscaled format)
121 u16 servoGetPositionRaw(u08 channel)
122 {
123 return ServoChannels[channel].duty;
124 }
125  
126 void servoService(void)
127 {
128 u16 nextTics;
129  
130 if(ServoChannel < SERVO_NUM_CHANNELS)
131 {
132 // turn off current channel
133 outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) & ~(ServoChannels[ServoChannel].pin));
134 }
135  
136 // next channel
137 ServoChannel++;
138  
139 if(ServoChannel != SERVO_NUM_CHANNELS)
140 {
141 // loop to channel 0 if needed
142 if(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0;
143 // turn on new channel
144 outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) | (ServoChannels[ServoChannel].pin));
145 // schedule turn off time
146 nextTics = ServoChannels[ServoChannel].duty;
147 }
148 else //(Channel == SERVO_NUM_CHANNELS)
149 {
150 // ***we could save time by precalculating this
151 // schedule end-of-period
152 nextTics = ServoPeriodTics-ServoPosTics;
153 }
154  
155 // schedule next interrupt
156 u16 OCValue;
157 // read in current value of output compare register OCR1A
158 OCValue = inb(OCR1AL); // read low byte of OCR1A
159 OCValue += inb(OCR1AH)<<8; // read high byte of OCR1A
160 // increment OCR1A value by nextTics
161 OCValue += nextTics;
162 // OCR1A+=nextTics;
163 // set future output compare time to this new value
164 outb(OCR1AH, (OCValue>>8)); // write high byte
165 outb(OCR1AL, (OCValue & 0x00FF)); // write low byte
166 // set our new tic position
167 ServoPosTics += nextTics;
168 if(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0;
169 }
{BLAME END}
{FOOTER START}

Powered by WebSVN v2.8.3