blob: 9a04d41b7100a29384c5e0ac46995eff79c52863 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
|
#include <avr/io.h>
#include <util/delay.h>
#include <util/delay.h>
#include <avr/interrupt.h>
/* 500Hz -> 2ms */
#define MAXPULSFREQ 500
/* timer1 value for a 2ms puls */
#define TIMER_MAXPULS F_CPU/MAXPULSFREQ
/* min/max values */
#define MINPULS 13500
#define MAXPULS 30000
/* baud rate for serial communication */
#define BAUD 9600UL
/* hardware settings for baud rate */
#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1)
#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1)))
#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD)
/* compile time check if baud rate is ok */
#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
#error systematic error of baud rate is > 1%
#endif
static uint16_t servo_pulslength[20];
static uint8_t activated = 0;
/* init IO pins */
static inline void init_io() {
/* PB1 = left:red, PB2 = left:green, PB3 = left:blue */
DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3);
/* PD3 = right:red, PD5 = right:green, PD6 = right:blue */
DDRD = (1 << PD3) | (1 << PD5) | (1 << PD6);
/* These pins are broken, so enable them */
DDRC = (1 << PC4) | (1 << PC5) | (1 << PC6);
PORTC = (1 << PC4) | (1 << PC5) | (1 << PC6);
}
/* INTERRUPT: timer for software pwm to control servos */
ISR(TIMER1_OVF_vect) {
static uint8_t servo_indexhalf = 0;
switch(servo_indexhalf) {
case 0 : PORTB |= (1 << PB1); break;
case 1 : PORTB &= ~(1 << PB1); break;
case 2 : PORTB |= (1 << PB2); break;
case 3 : PORTB &= ~(1 << PB2); break;
case 4 : PORTB |= (1 << PB3); break;
case 5 : PORTB &= ~(1 << PB3); break;
case 6 : PORTD |= (1 << PD3); break;
case 7 : PORTD &= ~(1 << PD3); break;
case 8 : PORTD |= (1 << PD5); break;
case 9 : PORTD &= ~(1 << PD5); break;
case 10: PORTD |= (1 << PD6); break;
case 11: PORTD &= ~(1 << PD6); break;
}
/* set time for the next interrupt */
TCNT1 = servo_pulslength[servo_indexhalf];
servo_indexhalf++;
if(servo_indexhalf == 20)
servo_indexhalf = 0;
}
/* setup serial communication */
static inline void init_uart() {
UBRR0 = UBRR_VAL;
UCSR0B |= (1 << TXEN0);
UCSR0B |= (1 << RXEN0);
/* Frame Format: Asynchron 8N1 */
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
}
/* set servo speed */
static inline void servo_set(uint8_t index, uint8_t value) {
if(!activated)
value = 0;
if (index < 6) {
uint16_t tmp = MINPULS+(MAXPULS-MINPULS)/256*value;
/* calculate hightime */
servo_pulslength[index<<1]=(-1)*tmp;
/* sum of low and hightime for one servo is 2ms */
servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-tmp);
}
}
/* init servos, reset speed */
static inline void init_servo() {
uint8_t i;
/* disable all motors */
for(i=0;i<6;i++)
servo_set(i, 0);
/* init timer */
TCNT1 = 0 - 16000;
TCCR1A = 0;
TCCR1B = (1 << CS10);
TIMSK1 |= (1 << TOIE1);
}
/* get a single byte from serial device */
static inline uint8_t uart_getc() {
while (!(UCSR0A & (1<<RXC0)));
return UDR0;
}
/* put a single byte to serial device */
static inline void uart_putc(uint8_t c) {
while (!(UCSR0A & (1<<UDRE0)));
UDR0 = c;
}
int main() {
/* initialize hardware */
init_io();
init_servo();
init_uart();
sei();
uint8_t c;
while(1) {
c = uart_getc();
/* set speed for single engine */
if(c >= 0x01 && c <= 0x06) {
uint8_t speed = uart_getc();
servo_set(c-1, speed);
/* set speed for all engines */
} else if(c == 0xFF) {
uint8_t speed = uart_getc();
for(int i=0;i<6;i++)
servo_set(i, speed);
/* activate engines */
} else if(c == 0xCA) {
if(uart_getc() != 0xFE) continue;
if(uart_getc() != 0xBA) continue;
if(uart_getc() != 0xBE) continue;
activated=1;
/* disable engines */
} else if(c == 0xDE) {
if(uart_getc() != 0xAD) continue;
if(uart_getc() != 0xBA) continue;
if(uart_getc() != 0xBE) continue;
activated=0;
for(int i=0;i<6;i++)
servo_set(i, 0);
/* ignore unknown commands */
} else if(c != 0x00) {
continue;
}
/* acknowledge command */
uart_putc('.');
}
return 0;
}
|
