1 | #include <avr/io.h>
|
2 | #include <avr/interrupt.h>
|
3 |
|
4 | #define Frequenz 100
|
5 | #define Frequenz2 1000
|
6 | #define Geschwindigkeit 500
|
7 | #define konstante1 25
|
8 | #define konstante2 50
|
9 |
|
10 | volatile int i=0;
|
11 | volatile int j=0;
|
12 | volatile int k=0;
|
13 | int SERVO_01=0,SERVO_02=0,SERVO_03=0,SERVO_11=0,SERVO_12=0,SERVO_13=0,SERVO_21=0,SERVO_22=0,SERVO_23=0,SERVO_31=0,SERVO_32=0,SERVO_33=0,SERVO_41=0,SERVO_42=0,SERVO_43=0,SERVO_51=0,SERVO_52=0,SERVO_53=0;
|
14 | int time0=0, time1=0, time2=0, time3=0, time4=0, time5=0, time6=0, time7=0, time8=0, time9=0, time10=0, time11=0, time12=0, time13=0, time14=0, time15=0, time16=0, time17=0;
|
15 | int Takt=0;
|
16 |
|
17 | ISR(TIMER0_COMP_vect) {
|
18 |
|
19 | i++;
|
20 | if (i==Frequenz) {
|
21 | i=0;
|
22 | }
|
23 |
|
24 | j++;
|
25 | if(j==Frequenz2) {
|
26 | j=0;
|
27 | }
|
28 |
|
29 | k++;
|
30 | if(k==6000) {
|
31 | k=0;
|
32 | }
|
33 |
|
34 | if(j==Geschwindigkeit) {
|
35 | Takt++;
|
36 | }
|
37 |
|
38 | }
|
39 |
|
40 | int main() {
|
41 |
|
42 | DDRA = 0b11111111;
|
43 | DDRB = 0b11111111;
|
44 | DDRC = 0b11111111;
|
45 | DDRD = 0b11111111;
|
46 |
|
47 | TCCR0 = (1<<WGM01) | (1<<CS01);
|
48 | TIMSK |= (1<<OCIE0);
|
49 | OCR0 = 200;
|
50 | sei();
|
51 |
|
52 | Stand();
|
53 |
|
54 | while(1) {
|
55 |
|
56 | Bewegung();
|
57 | POS_Berechnung();
|
58 | Servo_Ans();
|
59 | LED_Ans();
|
60 | }
|
61 | }
|
62 |
|
63 |
|
64 | void Bewegung() {
|
65 |
|
66 | //Takt 1:
|
67 |
|
68 | if(Takt==1) {
|
69 |
|
70 | SERVO_02=12;
|
71 | SERVO_42=18;
|
72 | SERVO_22=12;
|
73 | }
|
74 |
|
75 | //Takt 2:
|
76 |
|
77 | if(Takt==2) {
|
78 |
|
79 | SERVO_01=20;
|
80 | SERVO_41=10;
|
81 | SERVO_21=20;
|
82 | }
|
83 |
|
84 | //Takt 3:
|
85 |
|
86 | if(Takt==3) {
|
87 |
|
88 | SERVO_02=15;
|
89 | SERVO_42=15;
|
90 | SERVO_22=15;
|
91 | }
|
92 |
|
93 | //Takt 4:
|
94 |
|
95 | if(Takt==4) {
|
96 |
|
97 | SERVO_01=15;
|
98 | SERVO_41=15;
|
99 | SERVO_21=15;
|
100 |
|
101 | SERVO_52=18;
|
102 | SERVO_12=12;
|
103 | SERVO_32=18;
|
104 | }
|
105 |
|
106 | //Takt 5:
|
107 |
|
108 | if(Takt==5) {
|
109 | SERVO_51=10;
|
110 | SERVO_31=10;
|
111 | SERVO_11=20;
|
112 | }
|
113 |
|
114 | //Takt 6:
|
115 |
|
116 | if(Takt==6) {
|
117 | SERVO_52=15;
|
118 | SERVO_12=15;
|
119 | SERVO_32=15;
|
120 | }
|
121 |
|
122 | //Takt7:
|
123 |
|
124 | if(Takt==7) {
|
125 | SERVO_51=15;
|
126 | SERVO_31=15;
|
127 | SERVO_11=15;
|
128 | Takt=1;
|
129 | }
|
130 | }
|
131 |
|
132 | void Stand() {
|
133 |
|
134 | SERVO_01=15;
|
135 | SERVO_11=15;
|
136 | SERVO_21=15;
|
137 | SERVO_31=15;
|
138 | SERVO_41=15;
|
139 | SERVO_51=15;
|
140 | //-------------
|
141 | SERVO_02=15;
|
142 | SERVO_12=15;
|
143 | SERVO_22=15;
|
144 | SERVO_32=15;
|
145 | SERVO_42=15;
|
146 | SERVO_52=15;
|
147 | //-------------
|
148 | SERVO_03=15;
|
149 | SERVO_13=15;
|
150 | SERVO_23=15;
|
151 | SERVO_33=15;
|
152 | SERVO_43=15;
|
153 | SERVO_53=15;
|
154 | }
|
155 |
|
156 |
|
157 | void LED_Ans() {
|
158 |
|
159 | if(k==0) {
|
160 | PORTC = 0b11000000;
|
161 | }
|
162 |
|
163 | if(k==2000) {
|
164 | PORTC = 0b00110000;
|
165 | }
|
166 |
|
167 | if(k==4000) {
|
168 | PORTC = 0b00001100;
|
169 | }
|
170 |
|
171 | if(k==6000) {
|
172 | PORTC = 0b00000011;
|
173 | }
|
174 | }
|
175 |
|
176 |
|
177 | void POS_Berechnung() {
|
178 |
|
179 | time0=SERVO_01;
|
180 | time1=SERVO_11;
|
181 | time2=SERVO_21;
|
182 | time3=SERVO_31;
|
183 | time4=SERVO_41;
|
184 | time5=SERVO_51;
|
185 | //----------------------------
|
186 | time6=SERVO_02+konstante1;
|
187 | time7=SERVO_12+konstante1;
|
188 | time8=SERVO_22+konstante1;
|
189 | time9=SERVO_32+konstante1;
|
190 | time10=SERVO_42+konstante1;
|
191 | time11=SERVO_52+konstante1;
|
192 | //----------------------------
|
193 | time12=SERVO_03+konstante2;
|
194 | time13=SERVO_13+konstante2;
|
195 | time14=SERVO_23+konstante2;
|
196 | time15=SERVO_33+konstante2;
|
197 | time16=SERVO_43+konstante2;
|
198 | time17=SERVO_53+konstante2;
|
199 | }
|
200 |
|
201 | void Servo_Ans() {
|
202 |
|
203 | if((i>=0)&&(i<=25)) {
|
204 | if(i==0){
|
205 | PORTA = 0b00111111;
|
206 | }
|
207 |
|
208 | if(i==time0) {
|
209 | PORTA&=~(1<<PA0);
|
210 | }
|
211 |
|
212 | if(i==time1) {
|
213 | PORTA&=~(1<<PA1);
|
214 | }
|
215 |
|
216 | if(i==time2) {
|
217 | PORTA&=~(1<<PA2);
|
218 | }
|
219 |
|
220 | if(i==time3) {
|
221 | PORTA&=~(1<<PA3);
|
222 | }
|
223 |
|
224 | if(i==time4) {
|
225 | PORTA&=~(1<<PA4);
|
226 | }
|
227 |
|
228 | if(i==time5) {
|
229 | PORTA&=~(1<<PA5);
|
230 | }
|
231 | }
|
232 | //------------------------------------
|
233 | if((i>=25)&&(i<=50)) {
|
234 | if(i==25) {
|
235 | PORTB = 0b00111111;
|
236 | }
|
237 |
|
238 | if(i==time6) {
|
239 | PORTB&=~(1<<PB0);
|
240 | }
|
241 | if(i==time7) {
|
242 | PORTB&=~(1<<PB1);
|
243 | }
|
244 | if(i==time8) {
|
245 | PORTB&=~(1<<PB2);
|
246 | }
|
247 | if(i==time9) {
|
248 | PORTB&=~(1<<PB3);
|
249 | }
|
250 | if(i==time10) {
|
251 | PORTB&=~(1<<PB4);
|
252 | }
|
253 | if(i==time11) {
|
254 | PORTB&=~(1<<PB5);
|
255 | }
|
256 | }
|
257 | //----------------------------------
|
258 | if((i>=50)&&(i<=75)) {
|
259 | if(i==50) {
|
260 | PORTD = 0b00111111;
|
261 | }
|
262 |
|
263 | if(i==time12) {
|
264 | PORTD&=~(1<<PD0);
|
265 | }
|
266 | if(i==time13) {
|
267 | PORTD&=~(1<<PD1);
|
268 | }
|
269 | if(i==time14) {
|
270 | PORTD&=~(1<<PD2);
|
271 | }
|
272 | if(i==time15) {
|
273 | PORTD&=~(1<<PD3);
|
274 | }
|
275 | if(i==time16) {
|
276 | PORTD&=~(1<<PD4);
|
277 | }
|
278 | if(i==time17) {
|
279 | PORTD&=~(1<<PD5);
|
280 | }
|
281 | }
|
282 | }
|