Line No. | Rev | Author | Line |
---|---|---|---|
1 | 6 | kaklik | <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"> |
2 | <html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1"> |
||
3 | <title>Procyon AVRlib: servo.c Source File</title> |
||
4 | <link href="dox.css" rel="stylesheet" type="text/css"> |
||
5 | </head><body> |
||
6 | <!-- Generated by Doxygen 1.4.2 --> |
||
7 | <div class="qindex"><a class="qindex" href="main.html">Main Page</a> | <a class="qindex" href="modules.html">Modules</a> | <a class="qindex" href="annotated.html">Data Structures</a> | <a class="qindex" href="dirs.html">Directories</a> | <a class="qindex" href="files.html">File List</a> | <a class="qindex" href="functions.html">Data Fields</a> | <a class="qindex" href="globals.html">Globals</a> | <a class="qindex" href="pages.html">Related Pages</a></div> |
||
8 | <h1>servo.c</h1><a href="servo_8c.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment">00001 <span class="comment">/*! \file servo.c \brief Interrupt-driven RC Servo function library. */</span> |
||
9 | 00002 <span class="comment">//*****************************************************************************</span> |
||
10 | 00003 <span class="comment">//</span> |
||
11 | 00004 <span class="comment">// File Name : 'servo.c'</span> |
||
12 | 00005 <span class="comment">// Title : Interrupt-driven RC Servo function library</span> |
||
13 | 00006 <span class="comment">// Author : Pascal Stang - Copyright (C) 2002</span> |
||
14 | 00007 <span class="comment">// Created : 7/31/2002</span> |
||
15 | 00008 <span class="comment">// Revised : 8/02/2002</span> |
||
16 | 00009 <span class="comment">// Version : 1.0</span> |
||
17 | 00010 <span class="comment">// Target MCU : Atmel AVR Series</span> |
||
18 | 00011 <span class="comment">// Editor Tabs : 4</span> |
||
19 | 00012 <span class="comment">//</span> |
||
20 | 00013 <span class="comment">// This code is distributed under the GNU Public License</span> |
||
21 | 00014 <span class="comment">// which can be found at http://www.gnu.org/licenses/gpl.txt</span> |
||
22 | 00015 <span class="comment">//</span> |
||
23 | 00016 <span class="comment">//*****************************************************************************</span> |
||
24 | 00017 |
||
25 | 00018 <span class="preprocessor">#ifndef WIN32</span> |
||
26 | 00019 <span class="preprocessor"></span><span class="preprocessor"> #include <avr/io.h></span> |
||
27 | 00020 <span class="preprocessor">#endif</span> |
||
28 | 00021 <span class="preprocessor"></span> |
||
29 | 00022 <span class="preprocessor">#include "<a class="code" href="global_8h.html">global.h</a>"</span> |
||
30 | 00023 <span class="preprocessor">#include "<a class="code" href="servo_8h.html">servo.h</a>"</span> |
||
31 | 00024 |
||
32 | 00025 <span class="comment">// Program ROM constants</span> |
||
33 | 00026 |
||
34 | 00027 <span class="comment">// Global variables</span> |
||
35 | 00028 <span class="comment">// servo channel registers</span> |
||
36 | 00029 u16 ServoPosTics; |
||
37 | 00030 u16 ServoPeriodTics; |
||
38 | 00031 u08 ServoChannel; |
||
39 | 00032 ServoChannelType ServoChannels[SERVO_NUM_CHANNELS]; |
||
40 | 00033 |
||
41 | 00034 <span class="comment">// functions</span> |
||
42 | 00035 <span class="comment"></span> |
||
43 | 00036 <span class="comment">//! initializes software PWM system</span> |
||
44 | <a name="l00037"></a><a class="code" href="servo_8h.html#a1">00037</a> <span class="comment"></span><span class="keywordtype">void</span> <a class="code" href="servo_8c.html#a4">servoInit</a>(<span class="keywordtype">void</span>) |
||
45 | 00038 { |
||
46 | 00039 u08 channel; |
||
47 | 00040 <span class="comment">// disble the timer1 output compare A interrupt</span> |
||
48 | 00041 cbi(TIMSK, OCIE1A); |
||
49 | 00042 <span class="comment">// set the prescaler for timer1</span> |
||
50 | 00043 <a class="code" href="group__timer.html#ga6">timer1SetPrescaler</a>(<a class="code" href="group__timer.html#ga19">TIMER_CLK_DIV256</a>); |
||
51 | 00044 <span class="comment">// attach the software PWM service routine to timer1 output compare A</span> |
||
52 | 00045 <a class="code" href="group__timer.html#ga8">timerAttach</a>(TIMER1OUTCOMPAREA_INT, servoService); |
||
53 | 00046 <span class="comment">// enable and clear channels</span> |
||
54 | 00047 <span class="keywordflow">for</span>(channel=0; channel<SERVO_NUM_CHANNELS; channel++) |
||
55 | 00048 { |
||
56 | 00049 <span class="comment">// set minimum position as default</span> |
||
57 | 00050 ServoChannels[channel].duty = SERVO_MIN; |
||
58 | 00051 <span class="comment">// set default port and pins assignments</span> |
||
59 | 00052 ServoChannels[channel].port = _SFR_IO_ADDR(SERVO_DEFAULT_PORT); |
||
60 | 00053 <span class="comment">//ServoChannels[channel].port = (unsigned char)&SERVO_DEFAULT_PORT;</span> |
||
61 | 00054 ServoChannels[channel].pin = (1<<channel); |
||
62 | 00055 <span class="comment">// set channel pin to output</span> |
||
63 | 00056 <span class="comment">// THIS IS OBSOLETED BY THE DYNAMIC CHANNEL TO PORT,PIN ASSIGNMENTS</span> |
||
64 | 00057 <span class="comment">//outb(SERVODDR, inb(SERVODDR) | (1<<channel));</span> |
||
65 | 00058 } |
||
66 | 00059 <span class="comment">// set PosTics</span> |
||
67 | 00060 ServoPosTics = 0; |
||
68 | 00061 <span class="comment">// set PeriodTics</span> |
||
69 | 00062 ServoPeriodTics = SERVO_MAX*9; |
||
70 | 00063 <span class="comment">// set initial interrupt time</span> |
||
71 | 00064 u16 OCValue; |
||
72 | 00065 <span class="comment">// read in current value of output compare register OCR1A</span> |
||
73 | 00066 OCValue = inb(OCR1AL); <span class="comment">// read low byte of OCR1A</span> |
||
74 | 00067 OCValue += inb(OCR1AH)<<8; <span class="comment">// read high byte of OCR1A</span> |
||
75 | 00068 <span class="comment">// increment OCR1A value by nextTics</span> |
||
76 | 00069 OCValue += ServoPeriodTics; |
||
77 | 00070 <span class="comment">// set future output compare time to this new value</span> |
||
78 | 00071 outb(OCR1AH, (OCValue>>8)); <span class="comment">// write high byte</span> |
||
79 | 00072 outb(OCR1AL, (OCValue & 0x00FF)); <span class="comment">// write low byte</span> |
||
80 | 00073 <span class="comment">// enable the timer1 output compare A interrupt</span> |
||
81 | 00074 sbi(TIMSK, OCIE1A); |
||
82 | 00075 } |
||
83 | 00076 <span class="comment"></span> |
||
84 | 00077 <span class="comment">//! turns off software PWM system</span> |
||
85 | <a name="l00078"></a><a class="code" href="servo_8h.html#a2">00078</a> <span class="comment"></span><span class="keywordtype">void</span> <a class="code" href="servo_8c.html#a5">servoOff</a>(<span class="keywordtype">void</span>) |
||
86 | 00079 { |
||
87 | 00080 <span class="comment">// disable the timer1 output compare A interrupt</span> |
||
88 | 00081 cbi(TIMSK, OCIE1A); |
||
89 | 00082 <span class="comment">// detach the service routine</span> |
||
90 | 00083 <a class="code" href="group__timer.html#ga9">timerDetach</a>(TIMER1OUTCOMPAREA_INT); |
||
91 | 00084 } |
||
92 | 00085 <span class="comment"></span> |
||
93 | 00086 <span class="comment">//! set port and I/O pin for channel</span> |
||
94 | <a name="l00087"></a><a class="code" href="servo_8h.html#a3">00087</a> <span class="comment"></span><span class="keywordtype">void</span> <a class="code" href="servo_8c.html#a6">servoSetChannelIO</a>(u08 channel, u08 port, u08 pin) |
||
95 | 00088 { |
||
96 | 00089 ServoChannels[channel].port = port; |
||
97 | 00090 ServoChannels[channel].pin = (1<<(pin&0x07)); |
||
98 | 00091 } |
||
99 | 00092 <span class="comment"></span> |
||
100 | 00093 <span class="comment">//! set servo position on channel</span> |
||
101 | <a name="l00094"></a><a class="code" href="servo_8h.html#a4">00094</a> <span class="comment"></span><span class="keywordtype">void</span> <a class="code" href="servo_8c.html#a7">servoSetPosition</a>(u08 channel, u08 position) |
||
102 | 00095 { |
||
103 | 00096 <span class="comment">// input should be between 0 and SERVO_POSITION_MAX</span> |
||
104 | 00097 u16 pos_scaled; |
||
105 | 00098 <span class="comment">// calculate scaled position</span> |
||
106 | 00099 pos_scaled = ((u16)position*(SERVO_MAX-SERVO_MIN)/SERVO_POSITION_MAX)+SERVO_MIN; |
||
107 | 00100 <span class="comment">// set position</span> |
||
108 | 00101 <a class="code" href="servo_8c.html#a9">servoSetPositionRaw</a>(channel, pos_scaled); |
||
109 | 00102 } |
||
110 | 00103 <span class="comment"></span> |
||
111 | 00104 <span class="comment">//! get servo position on channel</span> |
||
112 | <a name="l00105"></a><a class="code" href="servo_8h.html#a5">00105</a> <span class="comment"></span>u08 <a class="code" href="servo_8c.html#a8">servoGetPosition</a>(u08 channel) |
||
113 | 00106 { |
||
114 | 00107 <span class="keywordflow">return</span> (u08)( ((<a class="code" href="servo_8c.html#a10">servoGetPositionRaw</a>(channel)-SERVO_MIN)*SERVO_POSITION_MAX)/(SERVO_MAX-SERVO_MIN) ); |
||
115 | 00108 } |
||
116 | 00109 <span class="comment"></span> |
||
117 | 00110 <span class="comment">//! set servo position on channel (raw unscaled format)</span> |
||
118 | <a name="l00111"></a><a class="code" href="servo_8h.html#a6">00111</a> <span class="comment"></span><span class="keywordtype">void</span> <a class="code" href="servo_8c.html#a9">servoSetPositionRaw</a>(u08 channel, u16 position) |
||
119 | 00112 { |
||
120 | 00113 <span class="comment">// bind to limits</span> |
||
121 | 00114 position = MAX(position, SERVO_MIN); |
||
122 | 00115 position = MIN(position, SERVO_MAX); |
||
123 | 00116 <span class="comment">// set position</span> |
||
124 | 00117 ServoChannels[channel].duty = position; |
||
125 | 00118 } |
||
126 | 00119 <span class="comment"></span> |
||
127 | 00120 <span class="comment">//! get servo position on channel (raw unscaled format)</span> |
||
128 | <a name="l00121"></a><a class="code" href="servo_8h.html#a7">00121</a> <span class="comment"></span>u16 <a class="code" href="servo_8c.html#a10">servoGetPositionRaw</a>(u08 channel) |
||
129 | 00122 { |
||
130 | 00123 <span class="keywordflow">return</span> ServoChannels[channel].duty; |
||
131 | 00124 } |
||
132 | 00125 |
||
133 | 00126 <span class="keywordtype">void</span> servoService(<span class="keywordtype">void</span>) |
||
134 | 00127 { |
||
135 | 00128 u16 nextTics; |
||
136 | 00129 |
||
137 | 00130 <span class="keywordflow">if</span>(ServoChannel < SERVO_NUM_CHANNELS) |
||
138 | 00131 { |
||
139 | 00132 <span class="comment">// turn off current channel</span> |
||
140 | 00133 outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) & ~(ServoChannels[ServoChannel].pin)); |
||
141 | 00134 } |
||
142 | 00135 |
||
143 | 00136 <span class="comment">// next channel</span> |
||
144 | 00137 ServoChannel++; |
||
145 | 00138 |
||
146 | 00139 <span class="keywordflow">if</span>(ServoChannel != SERVO_NUM_CHANNELS) |
||
147 | 00140 { |
||
148 | 00141 <span class="comment">// loop to channel 0 if needed</span> |
||
149 | 00142 <span class="keywordflow">if</span>(ServoChannel > SERVO_NUM_CHANNELS) ServoChannel = 0; |
||
150 | 00143 <span class="comment">// turn on new channel</span> |
||
151 | 00144 outb(_SFR_IO8(ServoChannels[ServoChannel].port), inb(_SFR_IO8(ServoChannels[ServoChannel].port)) | (ServoChannels[ServoChannel].pin)); |
||
152 | 00145 <span class="comment">// schedule turn off time</span> |
||
153 | 00146 nextTics = ServoChannels[ServoChannel].duty; |
||
154 | 00147 } |
||
155 | 00148 <span class="keywordflow">else</span> <span class="comment">//(Channel == SERVO_NUM_CHANNELS)</span> |
||
156 | 00149 { |
||
157 | 00150 <span class="comment">// ***we could save time by precalculating this</span> |
||
158 | 00151 <span class="comment">// schedule end-of-period</span> |
||
159 | 00152 nextTics = ServoPeriodTics-ServoPosTics; |
||
160 | 00153 } |
||
161 | 00154 |
||
162 | 00155 <span class="comment">// schedule next interrupt</span> |
||
163 | 00156 u16 OCValue; |
||
164 | 00157 <span class="comment">// read in current value of output compare register OCR1A</span> |
||
165 | 00158 OCValue = inb(OCR1AL); <span class="comment">// read low byte of OCR1A</span> |
||
166 | 00159 OCValue += inb(OCR1AH)<<8; <span class="comment">// read high byte of OCR1A</span> |
||
167 | 00160 <span class="comment">// increment OCR1A value by nextTics</span> |
||
168 | 00161 OCValue += nextTics; |
||
169 | 00162 <span class="comment">// OCR1A+=nextTics;</span> |
||
170 | 00163 <span class="comment">// set future output compare time to this new value</span> |
||
171 | 00164 outb(OCR1AH, (OCValue>>8)); <span class="comment">// write high byte</span> |
||
172 | 00165 outb(OCR1AL, (OCValue & 0x00FF)); <span class="comment">// write low byte</span> |
||
173 | 00166 <span class="comment">// set our new tic position</span> |
||
174 | 00167 ServoPosTics += nextTics; |
||
175 | 00168 <span class="keywordflow">if</span>(ServoPosTics >= ServoPeriodTics) ServoPosTics = 0; |
||
176 | 00169 } |
||
177 | </pre></div><hr size="1"><address style="align: right;"><small>Generated on Sun Oct 29 03:41:07 2006 for Procyon AVRlib by |
||
178 | <a href="http://www.doxygen.org/index.html"> |
||
179 | <img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.2 </small></address> |
||
180 | </body> |
||
181 | </html> |
Powered by WebSVN v2.8.3