1 | #define F_CPU 16000000UL
|
2 |
|
3 | #include <avr/io.h>
|
4 | #include <avr/interrupt.h>
|
5 | #include <util/delay.h>
|
6 |
|
7 | //
|
8 | // Der Prescaler muss so gewählt werden, dass der Ausdruck
|
9 | // für MILLISEC_BASE einen Wert kleiner als 128 ergibt
|
10 | // MILLISEC_BASE ist der Timerwert, der 1 Millisekunde Zeitdauer ergeben
|
11 | // soll.
|
12 | //
|
13 | #define PRESCALER 128
|
14 | #define PRESCALER_BITS (1<<CS12) | ( 1 << CS10)
|
15 |
|
16 | #define MILLISEC_BASE ( F_CPU / PRESCALER / 1000 )
|
17 | #define CENTER ( MILLISEC_BASE / 2 )
|
18 |
|
19 | //
|
20 | // Konfiguration der Servoleitungen
|
21 | //
|
22 | #define NR_SERVOS 8
|
23 | #define SERVO_DDR DDRD
|
24 | #define SERVO_PORT PORTD
|
25 | uint8_t ServoPuls[NR_SERVOS] = { 1<<PD0, 1<<PD1, 1<<PD2, 1<<PD3,
|
26 | 1<<PD4, 1<<PD5, 1<<PD6, 1<<PD7 };
|
27 | //
|
28 | // Werte für die Servoposition
|
29 | // Gültige Werte laufen von 0 bis 2 * CENTER
|
30 | // 0 ... ganz links
|
31 | // CENTER ... Mittelstellung
|
32 | // 2 * CENTER ... ganz rechts
|
33 | //
|
34 | volatile uint8_t ServoValue[NR_SERVOS];
|
35 |
|
36 | ISR (TIMER1_COMPB_vect)
|
37 | {
|
38 | static uint8_t ServoId = 0;
|
39 |
|
40 | //
|
41 | // den Puls des aktuellen Servos beenden
|
42 | //
|
43 | SERVO_PORT &= ~ServoPuls[ServoId];
|
44 |
|
45 | //
|
46 | // welches ist das nächste aktuelle Servo?
|
47 | //
|
48 | if( ++ServoId >= NR_SERVOS )
|
49 | ServoId = 0;
|
50 |
|
51 | //
|
52 | // die Ausgangsleitung fuer dieses Servo auf 1; den Puls beginnen
|
53 | //
|
54 | SERVO_PORT |= ServoPuls[ServoId];
|
55 |
|
56 | //
|
57 | // den Timer so einstellen, dass bei Pulsende, die ISR erneut aufgerufen wird
|
58 | //
|
59 | OCR1B = MILLISEC_BASE + ServoValue[ServoId];
|
60 | }
|
61 |
|
62 | void InitServo()
|
63 | {
|
64 | uint8_t i;
|
65 |
|
66 | //
|
67 | // Die Servoleitungen auf Ausgang stellen
|
68 | //
|
69 | SERVO_DDR = ServoPuls[0] | ServoPuls[1] | ServoPuls[2] | ServoPuls[3] |
|
70 | ServoPuls[4] | ServoPuls[5] | ServoPuls[6] | ServoPuls[7];
|
71 |
|
72 | //
|
73 | // Alle Servos in Mittelstellung
|
74 | //
|
75 | for( i = 0; i < NR_SERVOS; ++i )
|
76 | ServoValue[i] = CENTER;
|
77 |
|
78 | //
|
79 | // Timer auf CTC Modus konfigurieren
|
80 | //
|
81 | OCR1B = MILLISEC_BASE + ServoValue[0];
|
82 | TIMSK |= (1<<OCIE1A)|(1<<OCIE1B);
|
83 | TCCR1B = (1<<WGM12) | PRESCALER_BITS; // CTC mode
|
84 | }
|
85 |
|
86 | int main(void)
|
87 | {
|
88 | InitServo();
|
89 |
|
90 | sei();
|
91 |
|
92 | _delay_ms( 1000 );
|
93 |
|
94 | //
|
95 | // testweise einfach alle 8 Servos ansteuern
|
96 | // jedes Servo soll sich unterschiedlich schnell bewegen
|
97 | //
|
98 | while( 1 ) {
|
99 |
|
100 |
|
101 | ServoValue[0] += 2;
|
102 | if( ServoValue[0] > 2*CENTER )
|
103 | ServoValue[0] -= 2*CENTER;
|
104 |
|
105 | ServoValue[1] += 1;
|
106 | if( ServoValue[1] > 2*CENTER )
|
107 | ServoValue[1] -= 2*CENTER;
|
108 |
|
109 | ServoValue[2] += 2;
|
110 | if( ServoValue[2] > 2*CENTER )
|
111 | ServoValue[2] -= 2*CENTER;
|
112 |
|
113 | ServoValue[3] += 3;
|
114 | if( ServoValue[3] > 2*CENTER )
|
115 | ServoValue[3] -= 2*CENTER;
|
116 |
|
117 | ServoValue[4] += 1;
|
118 | if( ServoValue[4] > 2*CENTER )
|
119 | ServoValue[4] -= 2*CENTER;
|
120 |
|
121 | ServoValue[5] += 3;
|
122 | if( ServoValue[5] > 2*CENTER )
|
123 | ServoValue[5] -= 2*CENTER;
|
124 |
|
125 | ServoValue[6] += 2;
|
126 | if( ServoValue[6] > 2*CENTER )
|
127 | ServoValue[6] -= 2*CENTER;
|
128 |
|
129 | ServoValue[7] += 1;
|
130 | if( ServoValue[7] > 2*CENTER )
|
131 | ServoValue[7] -= 2*CENTER;
|
132 |
|
133 | _delay_ms( 40 );
|
134 | }
|
135 | }
|
Thread beobachten
Seitenaufteilung abschalten