1 | //
|
2 | // Programm fuer einen ATmega16
|
3 | //
|
4 | #define F_CPU 8000000UL
|
5 |
|
6 | #include <avr/io.h>
|
7 | #include <avr/interrupt.h>
|
8 | #include <util/delay.h>
|
9 |
|
10 | //
|
11 | // Der Prescaler muss so gewählt werden, dass der Ausdruck
|
12 | // für MILLISEC_BASE einen Wert kleiner als 128 ergibt
|
13 | // MILLISEC_BASE ist der Timerwert, der 1 Millisekunde Zeitdauer ergeben
|
14 | // soll.
|
15 | //
|
16 | #define PRESCALER 128
|
17 | #define PRESCALER_BITS (1<<CS22) | ( 1 << CS20 )
|
18 |
|
19 | #define MILLISEC_BASE ( F_CPU / PRESCALER / 1000 )
|
20 | #define CENTER ( MILLISEC_BASE / 2 )
|
21 |
|
22 | //
|
23 | // Konfiguration der Servoleitungen
|
24 | //
|
25 | #define NR_SERVOS 18
|
26 | #define GELENK1_DDR DDRD
|
27 | #define GELENK1_PORT PORTD
|
28 | #define GELENK2_DDR DDRC
|
29 | #define GELENK2_PORT PORTC
|
30 | #define GELENK3_DDR DDRB
|
31 | #define GELENK3_PORT PORTB
|
32 | uint8_t ServoPuls[NR_SERVOS] = { 1<<PD0, 1<<PD1, 1<<PD2, 1<<PD3,
|
33 | 1<<PD4, 1<<PD5,
|
34 | 1<<PC0, 1<<PC1, 1<<PC2, 1<<PC3,
|
35 | 1<<PC4, 1<<PC5,
|
36 | 1<<PB0, 1<<PB1, 1<<PB2, 1<<PB3,
|
37 | 1<<PB4, 1<<PB5 };
|
38 | //
|
39 | // Werte für die Servoposition
|
40 | // Gültige Werte laufen von 0 bis 2 * CENTER
|
41 | // 0 ... ganz links
|
42 | // CENTER ... Mittelstellung
|
43 | // 2 * CENTER ... ganz rechts
|
44 | //
|
45 | volatile uint8_t ServoValue[NR_SERVOS];
|
46 |
|
47 | ISR (TIMER2_COMP_vect)
|
48 | {
|
49 | static uint8_t ServoId = 0;
|
50 |
|
51 | //
|
52 | // den Puls des aktuellen Servos beenden
|
53 | //
|
54 | GELENK1_PORT &= ~ServoPuls[ServoId];
|
55 | GELENK2_PORT &= ~ServoPuls[ServoId];
|
56 | GELENK3_PORT &= ~ServoPuls[ServoId];
|
57 |
|
58 |
|
59 | //
|
60 | // welches ist das nächste aktuelle Servo?
|
61 | //
|
62 | if( ++ServoId >= NR_SERVOS )
|
63 | ServoId = 0;
|
64 |
|
65 | //
|
66 | // die Ausgangsleitung fuer dieses Servo auf 1; den Puls beginnen
|
67 | //
|
68 | GELENK1_PORT |= ServoPuls[ServoId];
|
69 | GELENK2_PORT |= ServoPuls[ServoId];
|
70 | GELENK3_PORT |= ServoPuls[ServoId];
|
71 |
|
72 | //
|
73 | // den Timer so einstellen, dass bei Pulsende, die ISR erneut aufgerufen wird
|
74 | //
|
75 | OCR2 = MILLISEC_BASE + ServoValue[ServoId];
|
76 | }
|
77 |
|
78 | void InitServo()
|
79 | {
|
80 | uint8_t i;
|
81 |
|
82 | //
|
83 | // Die Servoleitungen auf Ausgang stellen
|
84 | //
|
85 | GELENK1_DDR = ServoPuls[0] | ServoPuls[1] | ServoPuls[2] | ServoPuls[3] |
|
86 | ServoPuls[4] | ServoPuls[5] ;
|
87 | GELENK2_DDR = ServoPuls[6] | ServoPuls[7] | ServoPuls[8] | ServoPuls[9] |
|
88 | ServoPuls[10] | ServoPuls[11] ;
|
89 | GELENK3_DDR = ServoPuls[12] | ServoPuls[13] | ServoPuls[14] | ServoPuls[15] |
|
90 | ServoPuls[16] | ServoPuls[17] ;
|
91 |
|
92 | //
|
93 | // Alle Servos in Mittelstellung
|
94 | //
|
95 | for( i = 0; i < NR_SERVOS; ++i )
|
96 | ServoValue[i] = CENTER;
|
97 |
|
98 |
|
99 |
|
100 | //
|
101 | // Timer auf CTC Modus konfigurieren
|
102 | //
|
103 | OCR2 = MILLISEC_BASE + ServoValue[0];
|
104 | TIMSK |= (1<<OCIE2);
|
105 | TCCR2 = (1<<WGM21) | PRESCALER_BITS; // CTC mode
|
106 | }
|
107 |
|
108 | int main(void)
|
109 | {
|
110 | int x;
|
111 | InitServo();
|
112 |
|
113 | sei();
|
114 |
|
115 | _delay_ms( 1000 );
|
116 |
|
117 |
|
118 | while( 1 )
|
119 | {
|
120 | ServoValue[14] = 2*CENTER;
|
121 | }
|
122 | }
|