1 | #include <avr/io.h>
|
2 | #include <avr/interrupt.h>
|
3 | #include <inttypes.h>
|
4 | #include <string.h>
|
5 | #include <stdio.h>
|
6 | #include "uart.h"
|
7 |
|
8 | #define Servo_Port PORTD //PORT der Servo definieren
|
9 | #define Servo_quantity 7 //Servo Anzahl definieren
|
10 | #define Final_Position_high 40000 //obere Endlage definieren
|
11 | #define Final_Position_low 16000 //untere Endlage dafinieren
|
12 | #define F_CPU 20000000
|
13 |
|
14 | short Servo_Bit[Servo_quantity] = {1<<0, 1<<1, 1<<2, 1<<3, 1<<4, 1<<5, 1<<6, 1<<7}, Servo_ID=0, idling_cycle=136;
|
15 | int current_position[Servo_quantity]; //Variable IST Position
|
16 | long total_position=0; //Summer aller ausgebenen Impulse
|
17 |
|
18 | ISR(TIMER1_COMPA_vect) { //Timer1 ist mit Vorteiler0 initialisiert
|
19 | short i = 0;
|
20 |
|
21 | if(Servo_ID<=Servo_quantity) {
|
22 | Servo_Port = 0x00; //aktuelle Impüulsausgae beenden
|
23 | Servo_Port = Servo_Bit[Servo_ID]; //nächsten Impuls beginnen
|
24 | OCR1A = current_position[Servo_ID]; //die länge des Impulses in das Compare Register schreiben
|
25 | TCNT1 = 0; //das Zählregister zurrücksetzen
|
26 | Servo_ID++; //die Zählvariable erhöhen, damit beim nächsten Durchgang der nächste Pins gsetzt wird
|
27 | }else{
|
28 | for(i=0; i<7; i++) { //Die Summer aller Impulse eines Zyklus ausrechnen
|
29 | total_position+=current_position[i];
|
30 | }
|
31 | idling_cycle=200-total_position/2000; //Leerlaufzeit errechnen(damit die Frequenz immer 50Hz und nicht Impuls abhänig)
|
32 | TCCR0 |= (1<<CS01); //Timer0 starten
|
33 | TCCR1B &=~ (1<<CS10)|(1<<CS11)|(1<<CS12); //Timer1 stoppen
|
34 | TCNT1 = 0;
|
35 | }
|
36 | }
|
37 |
|
38 | ISR(TIMER0_COMP_vect){ //Timer0 ist so initialisiert, das jede 0,1ms ein overflow auftritt
|
39 | short i = 0;
|
40 |
|
41 | if(++i >= idling_cycle) { //idling_cycle legt die Durchgänge fest die nötig um auf 50Hz zu kommen
|
42 | Servo_ID = 0; //damit alles wieder von vorne beginnt Servo_ID auf nul setzen
|
43 | i = 0;
|
44 | TCCR1B |= (1<<CS10); //Timer1 starten
|
45 | TCCR0 &=~ (1<<CS01)|(1<<CS02)|(1<<CS00); //Timer0 beende
|
46 | }
|
47 | }
|
48 |
|
49 |
|
50 | int main(void) {
|
51 | short i = 0;
|
52 |
|
53 | for(i=0; i<7; i++) { //IST Positions Werte setzen (zum Testen alle gleich)
|
54 | current_position[i] = 16000;
|
55 | }
|
56 |
|
57 | Servo_Port = 0xFF;
|
58 | Servo_Port = 0x00;
|
59 | PORTB|=(1<<PB7);
|
60 | TCCR1B |= (1<<WGM12)|(1<<WGM13)|(1<<CS10); //Timer1 im CTC Modus mit Vorteiler 0 initialisieren
|
61 | TIMSK |= (1<<OCIE1A)|(1<<OCIE0); //Interrupt für Timer 0 und 1 freigeben
|
62 | ICR1=50000; //Top Wert für Timer1 festlegen
|
63 | TCCR0|=(1<<WGM01); //Timer0 im CTC Modus initialisieren
|
64 | OCR0=250-1; //OCR0 auf 250 setzen(entspricht einem Overflow alle 0,1ms bei 20MHz)
|
65 | sei();
|
66 |
|
67 | while(1) {
|
68 | }
|
69 |
|
70 | return(0);
|
71 | }
|