1 | /*
|
2 | * servo2.c
|
3 | *
|
4 | * Created on: 27.09.2010
|
5 | * Author: bernd
|
6 | */
|
7 |
|
8 | #include "servo2.h"
|
9 |
|
10 | byte position;
|
11 |
|
12 | void init_servo() {
|
13 |
|
14 | // ServoBits als Ausgang schalten
|
15 |
|
16 | servo_pin[0] = PC1;
|
17 | servo_pin[1] = PC2;
|
18 | #ifndef POLLIN
|
19 | servo_pin[2] = PD2;
|
20 | servo_pin[3] = PD3;
|
21 | servo_pin[4] = PD4;
|
22 | servo_pin[5] = PD5;
|
23 | servo_pin[6] = PD6;
|
24 | servo_pin[7] = PD7;
|
25 | #endif
|
26 | SERVO_0to1_DDR |= (1 << servo_pin[0]) | (1 << servo_pin[1]);
|
27 | #ifndef POLLIN
|
28 | SERVO_2to7_DDR |= (1 << servo_pin[2]) | (1 << servo_pin[3]) | (1
|
29 | << servo_pin[4]) | (1 << servo_pin[5]) | (1 << servo_pin[6]) | (1
|
30 | << servo_pin[7]);
|
31 | #endif
|
32 |
|
33 | #if defined(__AVR_ATmega48__) ||defined(__AVR_ATmega88__)
|
34 | // 8Bit-Timer initialisieren
|
35 | TIMSK0 |= (1 << TOIE0); // Overflow enable
|
36 | TCNT0 = 0;
|
37 | TCCR0A = (1 << CS02); // Prescaler 256
|
38 | // 16Bit Timer initialisieren
|
39 | TCCR1A = 0x00; // normaler Timermodus ( kein PWM )
|
40 | TCCR1B = 0x00; // Timer1 deaktivieren
|
41 | TIMSK1 |= (1 << TOIE1); // Timerüberlaufinterrupts aktivieren
|
42 | #endif
|
43 |
|
44 | #if defined(__AVR_ATmega8__)
|
45 | // 8Bit-Timer initialisieren
|
46 | TIMSK |= (1 << TOIE0); // Overflow enable
|
47 | TCNT0 = 0;
|
48 | TCCR0 = (1 << CS02); // Prescaler 256
|
49 | // 16Bit Timer initialisieren
|
50 | TCCR1A = 0x00; // normaler Timermodus ( kein PWM )
|
51 | TCCR1B = 0x00; // Timer1 deaktivieren
|
52 | TIMSK |= (1 << TOIE1); // Timerüberlaufinterrupts aktivieren
|
53 | #endif
|
54 |
|
55 | /**
|
56 | * Alle Servos reseten - Ausgang auschalten
|
57 | */
|
58 | for (int count = 0; count < MAX_SERVOS; count++) {
|
59 | servo_pos[count] = 0;
|
60 | if (count < 2) {
|
61 | SERVO_0to1_PORT &= ~(1 << servo_pin[count]);
|
62 | } else {
|
63 | SERVO_2to7_PORT &= ~(1 << servo_pin[count]);
|
64 | }
|
65 | }
|
66 |
|
67 | // TCNT0_DELAY = TIMER0_DELAY;
|
68 | servo_counter = 0;
|
69 | }
|
70 |
|
71 | /**
|
72 | * Setzt die Servoposition.
|
73 | *
|
74 | * Position ist ein Wert zw. 1-250.
|
75 | * 1 = Servosignal 1ms (links)
|
76 | * 250 = Servosignal 2ms (rechts)
|
77 | *
|
78 | * 8000000Mhz/sek = 8000Takte/ms
|
79 | * 8000Takte div 250Position = 32Takte/pos
|
80 | *
|
81 | * MAX_TIMER1 - 8000 = 57536 = 1ms bis Overflow
|
82 | * MAX_TIMER1 - 8000 - (32*position) = gewünschte Position
|
83 | */
|
84 | void setServoPos(byte servo, byte position) {
|
85 | if (servo > MAX_SERVOS - 1)
|
86 | servo = MAX_SERVOS - 1;
|
87 | if (position > MAX_SERVOPOS)
|
88 | position = MAX_SERVOPOS;
|
89 | if (position <=1)
|
90 | position = 0;
|
91 |
|
92 | servo_pos[servo] = (MAX_TIMER1 - (TIMER1_MS)) - (TICKS_PER_POS * position);
|
93 | }
|
94 |
|
95 | void setServoAngle(byte servo, byte degree) {
|
96 | if (degree < MIN_DEGREE)
|
97 | degree = MIN_DEGREE;
|
98 | if (degree > MAX_DEGREE)
|
99 | degree = MAX_DEGREE;
|
100 | if (servo > MAX_SERVOS - 1)
|
101 | servo = MAX_SERVOS - 1;
|
102 | //
|
103 | // Maximale position (MAX_SERVOPOS) / maximaler Winkel * gewünschter Winkel + 1
|
104 | // Ergebnis absolute Postion > 0 <= MAX_DEGREE
|
105 | //
|
106 | position = ((((MAX_SERVOPOS * 10) / MAX_DEGREE) * abs(degree) + 10) / 10);
|
107 | if (position == 0)
|
108 | position = 1;
|
109 | setServoPos(servo, position);
|
110 | }
|
111 |
|
112 | void deaktiateServoPorts(byte all, byte servo) {
|
113 | if (servo > MAX_SERVOS)
|
114 | all = 1;
|
115 | if (all > 0) {
|
116 | for (int count = 0; count < MAX_SERVOS; count++) {
|
117 | if (count < 2) {
|
118 | SERVO_0to1_PORT &= ~(1 << servo_pin[count]);
|
119 | } else {
|
120 | SERVO_2to7_PORT &= ~(1 << servo_pin[count]);
|
121 | }
|
122 | }
|
123 | } else {
|
124 | SERVO_0to1_PORT &= ~(1 << servo_pin[servo]);
|
125 | }
|
126 | }
|
127 |
|
128 | void aktiateServoPorts(byte all, byte servo) {
|
129 | //
|
130 | // Aus Sicherheitsgründen, falls der Servoparameter nicht stimmt
|
131 | // werden alle deaktiviert
|
132 | //
|
133 | if (servo > MAX_SERVOS)
|
134 | deaktiateServoPorts(all, servo);
|
135 | if (all > 0) {
|
136 | for (int count = 0; count < MAX_SERVOS; count++) {
|
137 | if (count < 2) {
|
138 | SERVO_0to1_PORT |= (1 << servo_pin[count]);
|
139 | } else {
|
140 | SERVO_2to7_PORT |= (1 << servo_pin[count]);
|
141 | }
|
142 | }
|
143 | } else {
|
144 | SERVO_2to7_PORT |= (1 << servo_pin[servo]);
|
145 | }
|
146 | }
|
147 |
|
148 | //----------------------------------------------------------------------------------------------
|
149 | // TIMER
|
150 | //----------------------------------------------------------------------------------------------
|
151 |
|
152 | /**
|
153 | * Timer 0 wird alle 2,5ms aufgerufen. Bei jedem Aufruf wird der nächste Servo
|
154 | * Pro Servo soll eine Periode von 20ms erreicht werden. Das heißt pro Servo
|
155 | * stehen 20/8 = 2,5ms zur Verfügung
|
156 | *
|
157 | * 8000000MHz = 8000Takte/ms div PreScaler256 = 31,25Takte/ms * 2,5ms = 78Takte
|
158 | * 256 - 78Takte = 178 TCCNT0
|
159 | * 8000/ms div 256 = 31,25/ms
|
160 | * Servo Periode = 20ms
|
161 | * MAX_SERVOS in 20ms = 20ms/8 = 2,5ms/pro Servo
|
162 | * Damit muss Timer0 alle 2,5ms einen Overflow INT bekommen.
|
163 | * Da ein Servo immer zw. 1-2ms angesteuert wird, funktioniert dies auch
|
164 | * Dauer: 6,5us - 7,1us
|
165 | */
|
166 | ISR(TIMER0_OVF_vect )
|
167 | {
|
168 | byte temp;
|
169 |
|
170 | temp = servo_counter;
|
171 | if (temp > MAX_SERVOS - 1)
|
172 | temp = 0;
|
173 |
|
174 | // nur wenn ServoPos > 0 den Servo aktivieren
|
175 | // ansonsten den Port abschalten
|
176 | //
|
177 | if (servo_pos[temp] > 0) {
|
178 | if (temp < 2) {
|
179 | SERVO_0to1_PORT |= (1 << servo_pin[temp]);
|
180 | } else {
|
181 | SERVO_2to7_PORT |= (1 << servo_pin[temp]);
|
182 | }
|
183 | } else {
|
184 | if (temp < 2) {
|
185 | SERVO_0to1_PORT &= ~(1 << servo_pin[temp]);
|
186 | } else {
|
187 | SERVO_2to7_PORT &= ~(1 << servo_pin[temp]);
|
188 | }
|
189 | }
|
190 | // TCNT1H = servo_pos[temp] >> 8;
|
191 | // TCNT1L = servo_pos[temp];
|
192 |
|
193 | // hier wird für den nächsten Servo der Timer 1 eingerichtet
|
194 | // TCNT1 enthält die Zeitspanne bis das Servosignal wieder auf LOW gehen soll
|
195 | TCNT1 = servo_pos[temp];
|
196 |
|
197 | // nächsten Servo
|
198 | servo_counter = ++temp;
|
199 |
|
200 | TCCR1B = (1 << CS10); // Timer1 aktivieren, Prescaler = 8
|
201 |
|
202 | //TCNT0 = TCNT0_DELAY; // Timer0 auf 178 setzen
|
203 | TCNT0 = 0xB2;
|
204 | }
|
205 |
|
206 | /**
|
207 | * Timing für diesen Timer 1
|
208 | *
|
209 | * Grundsätzlich löscht der Timer lediglich die beteiligten Ports
|
210 | * der Servos und setzt sie auf LOW. Timer 1 ist dafür
|
211 | * verantwortlich, dass der Servo 1-2ms angesteurt wird.
|
212 | *
|
213 | * Der Timerselber wird über den TIMER0 gesetzt.
|
214 | *
|
215 | * 8000000Mhz/sek = 8000Takte/ms
|
216 | * Prescaler = 1 = 8000Takte/ms
|
217 | * 65536 - 8000 = 57536 = TCCTN1 um 1ms bis Overflow
|
218 | * 65536 - 16000= 49536 = TCCNT1 um 2ms bis Overflow
|
219 | *
|
220 | * Der Timer1 läuft also immer zwischen 49536 - 57536 (zw. 1-2ms)
|
221 | * Dies entspricht dem Zeitverhalten eines Servos
|
222 | * Servo links 1ms, Servo rechts 2ms
|
223 | *
|
224 | */
|
225 | ISR(TIMER1_OVF_vect )
|
226 | {
|
227 | //
|
228 | // Alle Servo-Ports abschalten
|
229 | // Dauer: 6,25us
|
230 | for (byte counter = 0; counter < MAX_SERVOS; counter++) {
|
231 | if (counter < 2) {
|
232 | SERVO_0to1_PORT &= ~(1 << servo_pin[counter]);
|
233 | }
|
234 | #ifndef POLLIN
|
235 | else {
|
236 | SERVO_2to7_PORT &= ~(1 << servo_pin[counter]);
|
237 | }
|
238 | #endif
|
239 | }
|
240 | TCCR1B = 0x00;
|
241 | }
|