7b62c809209859236aba3c806f27386f3c6fbe94
[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 <util/twi.h>
9 #include <avr/eeprom.h>
10 #include <avr/wdt.h>
11 #include <avr/pgmspace.h>
12 #include "uart.h"
13
14 /*
15  * I2C Register Map (8 Bit)
16  * 0x00 Register select
17  * 0x01 Distance left MSB
18  * 0x02 Distance left LSB
19  * 0x03 Distance right MSB
20  * 0x04 Distance right LSB
21  * 0x05 Distance forward1 MSB
22  * 0x06 Distance forward1 LSB
23  * 0x07 Distance backward MSB
24  * 0x08 Distance backward LSB
25  * 0x09 Voltage MSB
26  * 0x0A Voltage LSB
27  * 0x0B Distance forward2 MSB
28  * 0x0C Distance forward2 LSB
29  *
30  * 0x15 Distance forward1 MSB (read only)
31  * 0x16 Distance forward1 LSB (read only)
32  * 0x17 Distance backward MSB (read only)
33  * 0x18 Distance backward LSB (read only)
34  * 0x19 Distance forward2 MSB (read only)
35  * 0x1A Distance forward2 LSB (read only)
36  *
37  * 0xff Bootloader
38  */
39
40
41 #define TWI_ACK   TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWEN) | (1<<TWIE)
42 #define TWI_NAK   TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
43 #define TWI_RESET TWCR = (1<<TWINT) | (1<<TWEA) | (1<<TWSTO) | (1<<TWEN) | (1<<TWIE);
44
45 static volatile uint8_t ireg=0;
46 static volatile uint8_t bootloader=0;
47 static volatile uint16_t dist_left=0;
48 static volatile uint16_t dist_right=0;
49 static volatile uint16_t dist_forward1=0;
50 static volatile uint16_t dist_forward2=0;
51 static volatile uint16_t dist_backward=0;
52 static volatile uint8_t start_dist_fwd1=0;
53 static volatile uint8_t start_dist_fwd2=0;
54 static volatile uint8_t start_dist_bwd=0;
55 static volatile uint16_t voltage=0;
56 static volatile uint8_t pind_pre=0;
57
58 ISR(TWI_vect)
59 {
60         static int16_t tmp16=0;
61
62         switch(TW_STATUS)
63         {  
64                 case TW_SR_SLA_ACK: // start write
65                         TWI_ACK;
66                         ireg = 0;
67                         break;
68                 case TW_SR_DATA_ACK: // write
69                         switch(ireg) {
70                                 case 0x00: // register select
71                                         ireg = TWDR;
72
73                                         if (ireg == 0x05) start_dist_fwd1=1;
74                                         if (ireg == 0x07) start_dist_bwd=1;
75                                         if (ireg == 0x0b) start_dist_fwd2=1;
76
77                                         ireg--; // because we do ireg++ below
78                                         TWI_ACK;
79                                         break;
80                                 case 0xff: // bootloader
81                                         bootloader = TWDR;
82                                 default:
83                                         TWI_NAK;
84                         }
85                         if (ireg < 0xff) ireg++;
86                         break;
87                 case TW_ST_SLA_ACK: // start read
88                 case TW_ST_DATA_ACK: // read
89                         switch(ireg) {
90                                 case 0x01: // Distance left MSB
91                                         tmp16 = dist_left;
92                                         TWDR = tmp16>>8;
93                                         TWI_ACK;
94                                         break;
95                                 case 0x02: // Distance right LSB
96                                         TWDR = tmp16;
97                                         TWI_ACK;
98                                         break;
99                                 case 0x03: // Distance right MSB
100                                         tmp16 = dist_right;
101                                         TWDR = tmp16>>8;
102                                         TWI_ACK;
103                                         break;
104                                 case 0x04: // Distance right LSB
105                                         TWDR = tmp16;
106                                         TWI_ACK;
107                                         break;
108                                 case 0x05: // Distance forward1 MSB
109                                         tmp16 = dist_forward1;
110                                         TWDR = tmp16>>8;
111                                         TWI_ACK;
112                                         break;
113                                 case 0x06: // Distance forward1 LSB
114                                         TWDR = tmp16;
115                                         TWI_ACK;
116                                         break;
117                                 case 0x07: // Distance backward MSB
118                                         tmp16 = dist_backward;
119                                         TWDR = tmp16>>8;
120                                         TWI_ACK;
121                                         break;
122                                 case 0x08: // Distance backward LSB
123                                         TWDR = tmp16;
124                                         TWI_ACK;
125                                         break;
126                                 case 0x09: // Voltage MSB
127                                         tmp16 = voltage;
128                                         TWDR = tmp16>>8;
129                                         TWI_ACK;
130                                         break;
131                                 case 0x0A: // Voltage LSB
132                                         TWDR = tmp16;
133                                         TWI_ACK;
134                                         break;
135                                 case 0x0B: // Distance forward2 MSB
136                                         tmp16 = dist_forward2;
137                                         TWDR = tmp16>>8;
138                                         TWI_ACK;
139                                         break;
140                                 case 0x0C: // Distance forward2 LSB
141                                         TWDR = tmp16;
142                                         TWI_ACK;
143                                         break;
144                                 case 0x15: // Distance forward1 MSB
145                                         tmp16 = dist_forward1;
146                                         TWDR = tmp16>>8;
147                                         TWI_ACK;
148                                         break;
149                                 case 0x16: // Distance forward1 LSB
150                                         TWDR = tmp16;
151                                         TWI_ACK;
152                                         break;
153                                 case 0x17: // Distance backward MSB
154                                         tmp16 = dist_backward;
155                                         TWDR = tmp16>>8;
156                                         TWI_ACK;
157                                         break;
158                                 case 0x18: // Distance backward LSB
159                                         TWDR = tmp16;
160                                         TWI_ACK;
161                                         break;
162                                 case 0x19: // Distance forward2 MSB
163                                         tmp16 = dist_forward2;
164                                         TWDR = tmp16>>8;
165                                         TWI_ACK;
166                                         break;
167                                 case 0x1A: // Distance forward2 LSB
168                                         TWDR = tmp16;
169                                         TWI_ACK;
170                                         break;
171                                 default:
172                                         TWDR = 0;
173                                         TWI_NAK;
174                         }
175                         ireg++;
176                         break;
177                 case TW_SR_STOP:
178                         TWI_ACK;
179                         break;
180                 default:
181                         TWI_RESET;
182         }
183 }
184
185
186 uint16_t ReadChannel(uint8_t mux) {
187         uint8_t i;
188         uint16_t result;
189
190         ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // Frequenzvorteiler 
191         // setzen auf 8 (1) und ADC aktivieren (1)
192
193         ADMUX = mux;                      // Kanal waehlen
194         ADMUX |= (1<<REFS0);
195
196         /* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
197            also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
198         ADCSRA |= (1<<ADSC);              // eine ADC-Wandlung 
199         while ( ADCSRA & (1<<ADSC)  ) {
200                      // auf Abschluss der Konvertierung warten 
201         }
202         result = ADCW;  // ADCW muss einmal gelesen werden,
203         // sonst wird Ergebnis der nächsten Wandlung
204         // nicht übernommen.
205
206         /* Eigentliche Messung - Mittelwert aus 5 aufeinanderfolgenden Wandlungen */
207         result = 0;
208         for( i=0; i<5; i++ )
209         {
210                 ADCSRA |= (1<<ADSC);            // eine Wandlung "single conversion"
211                 while ( ADCSRA & (1<<ADSC) ) {
212                            // auf Abschluss der Konvertierung warten
213                 }
214                 result += ADCW;             // Wandlungsergebnisse aufaddieren
215         }
216
217         ADCSRA &= ~(1<<ADEN);             // ADC deaktivieren (2)
218
219         result /= 5;                     // Summe durch 5 teilen = arithm. Mittelwert
220         
221         return result;
222 }
223
224
225 static unsigned short get_distance(uint8_t i) {
226         return ReadChannel(i);
227 }
228
229
230 static unsigned short get_voltage(void) {
231         return ReadChannel(2)*1.46;
232 }
233
234
235 ISR(INT0_vect) {
236         static uint16_t t_start=0;
237         uint16_t t_now = TCNT1;
238         uint16_t t_diff;
239
240         if (bit_is_set(PIND, 2)) { // high level
241                 // start timer
242                 t_start = t_now;
243         } else {
244                 t_diff = t_now - t_start;
245                 dist_forward1 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
246                 // disable this interrupt
247                 EIMSK &= ~(1 << INT0);
248         }
249 }
250
251
252 ISR(INT1_vect) {
253         static uint16_t t_start=0;
254         uint16_t t_now = TCNT1;
255         uint16_t t_diff;
256
257         if (bit_is_set(PIND, 3)) { // high level
258                 // start timer
259                 t_start = t_now;
260         } else {
261                 t_diff = t_now - t_start;
262                 dist_backward = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
263                 // disable this interrupt
264                 EIMSK &= ~(1 << INT1);
265         }
266 }
267
268
269 ISR(PCINT2_vect) {
270         uint8_t pind_cur = PIND;
271
272         if ((pind_cur ^ pind_pre) & (1<<4)) { // PCINT20
273                 static uint16_t t_start=0;
274                 uint16_t t_now = TCNT1;
275                 uint16_t t_diff;
276
277                 if (bit_is_set(pind_cur, 4)) { // high level
278                         // start timer
279                         t_start = t_now;
280                 } else {
281                         t_diff = t_now - t_start;
282                         dist_forward2 = t_diff*2.7586 + 0.5; // t [µs] / 580 = mm
283                         // disable this interrupt
284                         PCMSK2 &= ~(1 << PCINT20);
285                 }
286         }
287
288         pind_pre = pind_cur;
289 }
290
291
292 int main(void) {
293         bootloader = 0x00;
294         setup_uart(9600);
295         uart_setup_stdout();
296
297         // I2C
298         TWAR = 0x52;
299         TWI_ACK;
300
301         // Timer 1: Normal mode, Top: 0xffff, Prescaler: F_CPU/256=62500Hz
302         TCCR1A = 0x0;
303         TCCR1B = (1 << CS12);
304
305         // External Interrupts
306         EICRA = (1 << ISC10) | (1 << ISC00);
307         PCICR = (1 << PCIE2);
308
309         printf_P(PSTR("\r\nStart\r\n"));
310
311         set_sleep_mode(SLEEP_MODE_IDLE);
312         sei();
313         while(1) {
314                 switch(ireg) {
315                         case 0x01: // ir left
316                                 dist_left = get_distance(0);
317                                 break;
318                         case 0x03: // ir right
319                                 dist_right = get_distance(1);
320                                 break;
321                         case 0x09: // voltage
322                                 voltage = get_voltage();
323                                 break;
324                         case 0xff: // Magic reg that starts the bootloader
325                                 if (bootloader == 0xa5) {
326                                         cli();
327                                         // write mark to first area in eeprom
328                                         eeprom_write_byte((uint8_t*)0, 123);
329                                         eeprom_busy_wait();
330                                         // Use watchdog to restart
331                                         wdt_enable(WDTO_15MS);
332                                 }
333                                 break;
334                 }
335
336                 if (start_dist_fwd1) {
337                         start_dist_fwd1 = 0;
338                         dist_forward1 = 0;
339
340                         DDRD |= (1 << 2);
341                         PORTD |= (1 << 2);
342                         _delay_us(10);
343                         PORTD &= ~(1 << 2);
344                         DDRD &= ~(1 << 2);
345                         // wait for interrupt
346                         EIFR &= (1 << INTF0); // clear old interrupt before enabling
347                         EIMSK |= (1 << INT0);
348                 }
349                 if (start_dist_bwd) {
350                         start_dist_bwd = 0;
351                         dist_backward = 0;
352
353                         DDRD |= (1 << 3);
354                         PORTD |= (1 << 3);
355                         _delay_us(10);
356                         PORTD &= ~(1 << 3);
357                         DDRD &= ~(1 << 3);
358                         // wait for interrupt
359                         EIFR &= (1 << INTF1); // clear old interrupt before enabling
360                         EIMSK |= (1 << INT1);
361                 }
362                 if (start_dist_fwd2) {
363                         start_dist_fwd2 = 0;
364                         dist_forward2 = 0;
365
366                         // PD4 = PCINT20
367                         DDRD |= (1 << 4);
368                         PORTD |= (1 << 4);
369                         _delay_us(10);
370                         PORTD &= ~(1 << 4);
371                         DDRD &= ~(1 << 4);
372                         // wait for interrupt
373                         pind_pre = PIND;
374                         PCMSK2 |= (1 << PCINT20);
375                 }
376
377                 sleep_mode();
378         }
379
380         return 0;
381 }