51cd1450d68e1fab6cc981bb5cb8a9bcd8dbe790
[ros_wild_thumper.git] / avr / nano / main.c
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <limits.h>
4 #include <avr/io.h>
5 #include <avr/interrupt.h>
6 #include <avr/sleep.h>
7 #include <util/delay.h>
8 #include "uart.h"
9
10 /*
11  * I2C Register Map (8 Bit)
12  * 0x00 Register select
13  * 0x01 Distance left MSB
14  * 0x02 Distance left LSB
15  * 0x03 Distance right MSB
16  * 0x04 Distance right LSB
17  * 0x05 Distance forward MSB
18  * 0x06 Distance forward LSB
19  * 0x07 Distance backward MSB
20  * 0x08 Distance backward LSB
21  * 0x15 Distance forward MSB (read only)
22  * 0x16 Distance forward LSB (read only)
23  * 0x17 Distance backward MSB (read only)
24  * 0x18 Distance backward LSB (read only)
25  * 0x09 Voltage MSB
26  * 0x0A Voltage LSB
27  *
28  * 0xff Bootloader
29  */
30
31
32 #define TWI_ACK         TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
33 #define TWI_RESET       TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
34 #define TWI_NAK         TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
35
36 static volatile uint8_t ireg=0;
37 static volatile uint8_t bootloader=0;
38 static volatile uint16_t dist_left=0;
39 static volatile uint16_t dist_right=0;
40 static volatile uint16_t dist_forward=0;
41 static volatile uint16_t dist_backward=0;
42 static volatile uint8_t start_dist_fwd=0;
43 static volatile uint8_t start_dist_bwd=0;
44 static volatile uint16_t voltage=0;
45
46 ISR(TWI_vect)
47 {
48         static int16_t tmp16=0;
49
50         switch (TWSR & 0xF8)
51         {  
52                 case 0x60: // start write
53                         TWI_ACK;
54                         ireg = 0;
55                         break;
56                 case 0x80: // write
57                         switch(ireg) {
58                                 case 0x00: // register select
59                                         ireg = TWDR;
60
61                                         if (ireg == 0x05) start_dist_fwd=1;
62                                         if (ireg == 0x07) start_dist_bwd=1;
63
64                                         ireg--; // because we do ireg++ below
65                                         TWI_ACK;
66                                         break;
67                                 case 0xff: // bootloader
68                                         bootloader = TWDR;
69                                 default:
70                                         TWI_NAK;
71                         }
72                         ireg++;
73                         break;
74                 case 0xA8: // start read
75                 case 0xB8: // read
76                         switch(ireg) {
77                                 case 0x01: // Distance left MSB
78                                         tmp16 = dist_left;
79                                         TWDR = tmp16>>8;
80                                         TWI_ACK;
81                                         break;
82                                 case 0x02: // Distance right LSB
83                                         TWDR = tmp16;
84                                         TWI_ACK;
85                                         break;
86                                 case 0x03: // Distance right MSB
87                                         tmp16 = dist_right;
88                                         TWDR = tmp16>>8;
89                                         TWI_ACK;
90                                         break;
91                                 case 0x04: // Distance right LSB
92                                         TWDR = tmp16;
93                                         TWI_ACK;
94                                         break;
95                                 case 0x05: // Distance forward MSB
96                                         tmp16 = dist_forward;
97                                         TWDR = tmp16>>8;
98                                         TWI_ACK;
99                                         break;
100                                 case 0x06: // Distance forward LSB
101                                         TWDR = tmp16;
102                                         TWI_ACK;
103                                         break;
104                                 case 0x07: // Distance backward MSB
105                                         tmp16 = dist_backward;
106                                         TWDR = tmp16>>8;
107                                         TWI_ACK;
108                                         break;
109                                 case 0x08: // Distance backward LSB
110                                         TWDR = tmp16;
111                                         TWI_ACK;
112                                         break;
113                                 case 0x15: // Distance forward MSB
114                                         tmp16 = dist_forward;
115                                         TWDR = tmp16>>8;
116                                         TWI_ACK;
117                                         break;
118                                 case 0x16: // Distance forward LSB
119                                         TWDR = tmp16;
120                                         TWI_ACK;
121                                         break;
122                                 case 0x17: // Distance backward MSB
123                                         tmp16 = dist_backward;
124                                         TWDR = tmp16>>8;
125                                         TWI_ACK;
126                                         break;
127                                 case 0x18: // Distance backward LSB
128                                         TWDR = tmp16;
129                                         TWI_ACK;
130                                         break;
131                                 case 0x09: // Voltage MSB
132                                         tmp16 = voltage;
133                                         TWDR = tmp16>>8;
134                                         TWI_ACK;
135                                         break;
136                                 case 0x0A: // Voltage LSB
137                                         TWDR = tmp16;
138                                         TWI_ACK;
139                                         break;
140                                 default:
141                                         TWDR = 0;
142                                         TWI_NAK;
143                         }
144                         ireg++;
145                         break;
146                 default:
147                         TWI_RESET;
148         }
149 }
150
151
152 uint16_t ReadChannel(uint8_t mux) {
153         uint8_t i;
154         uint16_t result;
155
156         ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler 
157         // setzen auf 8 (1) und ADC aktivieren (1)
158
159         ADMUX = mux;                      // Kanal waehlen
160         ADMUX |= (1<<REFS0);
161
162         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
163            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
164         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
165         while ( ADCSRA & (1<<ADSC)  ) {
166                      // auf Abschluss der Konvertierung warten 
167         }
168         result = ADCW;  // ADCW muss einmal gelesen werden,
169         // sonst wird Ergebnis der nächsten Wandlung
170         // nicht übernommen.
171
172         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
173         result = 0;
174         for( i=0; i<5; i++ )
175         {
176                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
177                 while ( ADCSRA & (1<<ADSC) ) {
178                            // auf Abschluss der Konvertierung warten
179                 }
180                 result += ADCW;             // Wandlungsergebnisse aufaddieren
181         }
182
183         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
184
185         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
186         
187         return result;
188 }
189
190
191 static unsigned short get_distance(uint8_t i) {
192         return ReadChannel(i);
193 }
194
195
196 static unsigned short get_voltage(void) {
197         return ReadChannel(2)*1.46;
198 }
199
200
201 ISR(INT0_vect) {
202         static uint16_t t_start=0;
203         uint16_t t_now = TCNT1;
204         uint16_t t_diff;
205
206         if (bit_is_set(PIND, 2)) { // high level
207                 // start timer
208                 t_start = t_now;
209         } else {
210                 t_diff = t_now - t_start;
211                 dist_forward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
212                 // disable this interrupt
213                 EIMSK |= (1 << INT0);
214         }
215 }
216
217
218 ISR(INT1_vect) {
219         static uint16_t t_start=0;
220         uint16_t t_now = TCNT1;
221         uint16_t t_diff;
222
223         if (bit_is_set(PIND, 3)) { // high level
224                 // start timer
225                 t_start = t_now;
226         } else {
227                 t_diff = t_now - t_start;
228                 dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
229                 // disable this interrupt
230                 EIMSK |= (1 << INT1);
231         }
232 }
233
234
235 int main(void) {
236         bootloader = 0x00;
237         setup_uart(9600);
238         uart_setup_stdout();
239
240         // I2C
241         TWAR = 0x52;
242         TWI_RESET;
243
244         // Timer 1: Normal mode, Top: 0xffff, Prescaler: F_CPU/256=62500Hz
245         TCCR1A = 0x0;
246         TCCR1B = (1 << CS12);
247
248         // External Interrupts
249         EICRA = (1 << ISC10) | (1 << ISC00);
250
251         printf("\r\nStart\r\n");
252
253         set_sleep_mode(SLEEP_MODE_IDLE);
254         sei();
255         while(1) {
256                 switch(ireg) {
257                         case 0x01: // ir left
258                                 dist_left = get_distance(0);
259                                 break;
260                         case 0x03: // ir right
261                                 dist_right = get_distance(1);
262                                 break;
263                         case 0x09: // voltage
264                                 voltage = get_voltage();
265                                 break;
266                         case 0xff: // Magic reg that starts the bootloader
267                                 if (bootloader == 0xa5) {
268                                         cli();
269                                         {
270                                                 void (*start)(void) = (void*)0x1800;
271                                                 start();
272                                         }
273                                 }
274                                 break;
275                 }
276
277                 if (start_dist_fwd) {
278                         start_dist_fwd = 0;
279                         dist_forward = 0;
280
281                         DDRD |= (1 << 2);
282                         PORTD |= (1 << 2);
283                         _delay_us(10);
284                         PORTD &= ~(1 << 2);
285                         DDRD &= ~(1 << 2);
286                         // wait for interrupt
287                         EIMSK |= (1 << INT0);
288                 }
289                 if (start_dist_bwd) {
290                         start_dist_bwd = 0;
291                         dist_backward = 0;
292
293                         DDRD |= (1 << 3);
294                         PORTD |= (1 << 3);
295                         _delay_us(10);
296                         PORTD &= ~(1 << 3);
297                         DDRD &= ~(1 << 3);
298                         // wait for interrupt
299                         EIMSK |= (1 << INT1);
300                 }
301
302                 sleep_mode();
303         }
304
305         return 0;
306 }